卡尔曼滤波

原理：1、加权平均数。2、状态空间方程

例子：两个人测同一个数据，甲测出来是满足正态分布，均值，方差；乙测出来是满足正态分布，均值，方差，那么这个数据量x应该是多少呢？

取一个加权平均数：  

那么这个数的方差:  

所以现在要求，使得取得最小值，那么对这个方程求导并等于0，求出当时使得x方差最小。

如果又测量了，那么继续用和继续加权计算不用保留前面数据导致计算量过大，可以实时计算。

状态转移方程和观测方程方程：

                                            (1)

                                                    (2)

                其中是状态转移矩阵，是观测方程，是噪声

设初始方差为，方差为，方差为

经典卡尔曼滤波的五个方程：

                                                (3)

                                              (4)

                                             (5)

                                                 (6)

                                                                      (7-1)

                                            (7-2)

                其中式3是状态变量从k-1到k时刻的预测值；式4对应其协方差矩阵；式5求卡尔曼增益，其中可以称为观测方程的协方差；式6更新状态变量后验值，称为残差；式7-2更新状态变量的协方差，式7-1会有数值计算问题，可能会有奇异性，会发散。

下面是一个简单的用卡尔曼滤波求最优解的cpp例子：理想自由落体运动，1、2、3、4、5秒观测的位置是4.9、19.6、44.1、78.7、122.5，求重力加速度。

#include <Eigen/Dense>
#include <iostream>
#include <vector>
#include "matplotlibcpp.h"

using namespace Eigen;
using namespace std;
namespace plt = matplotlibcpp;

int main() {
    VectorXd h(5);
    h << 4.9, 19.6, 44.1, 78.4, 122.5;//观测量
    MatrixXd X(3, 1);//状态变量，位置、速度、加速度
    X << 0, 0, 10;//初始状态变量的值
    MatrixXd Phi0(3, 3);//状态转移矩阵
    Phi0 << 1, 0.001, 0,
            0, 1, 0.001,
            0, 0, 1;//初始状态转移矩阵值    
    MatrixXd Phi = MatrixXd::Identity(3, 3);//单位阵    
    MatrixXd P = MatrixXd::Zero(3, 3);//状态变量的协方差
    P.diagonal() << 0, 0, 0.01;//主对角线的值
    MatrixXd Q = MatrixXd::Zero(3, 3);//状态变量噪声的协方差
    double R = 0.0001;//观测噪声的方差
    RowVectorXd H(3);//观测矩阵
    H << 1, 0, 0;//观测位置

    vector<MatrixXd> data(6, MatrixXd(3, 1));
    data[0] = X;

    for (int n = 1; n <= 5000; ++n) {
        Phi = Phi0 * Phi;//状态转移过程
        if (n % 1000 == 0) {
            MatrixXd Pkk = Phi * P * Phi.transpose() + Q;//预测状态变量的协方差
            double S = (H * Pkk * H.transpose())(0, 0) + R; // 计算S, 是1x1矩阵的第一个元素加R，观测的协方差
            MatrixXd K = Pkk * H.transpose() / S; // 标量除法，计算卡尔曼增益
            P = (MatrixXd::Identity(3, 3) - K * H) * Pkk*(MatrixXd::Identity(3, 3) - K * H) .transpose()+K*R*K.transpose();//更新状态变量的协方差
            MatrixXd Xkk = Phi * X;//状态变量预测值
            double z = h((n / 1000) - 1);//观测值
            X = Xkk + K * (z - (H * Xkk)(0, 0)); // 使用(0, 0)提取矩阵第一个元素，优化残差
            data[n / 1000] = X;
            Phi = MatrixXd::Identity(3, 3);//每次都要初始化是因为状态变量的值是在有观测值的时候更新的，在观测阶段保持的状态变量值在进行下一个状态的估计时，状态转移矩阵需要从零开始
        }
    }

    // Extract data for plotting
    vector<double> plot_data;
    for (const auto& d : data) {
        plot_data.push_back(d(2, 0));  // Third component
    }

    plt::plot(plot_data);
    plt::title("Kalman Filter Results");
    plt::show();

    return 0;
}

        在卡尔曼滤波的过程中，状态变量设置的是[位置、速度、加速度]，有一点值得注意的是我只用一个位置观测量却优化得到了我想要的加速度后验值，这一点是由状态空间方程的性质决定的，每个变量之间互相牵连；最终的后验值是由卡尔曼增益决定的，而卡尔曼增益又受状态变量的协方差P影响，P又受状态转移矩阵的影响，状态转移矩阵提供了各个状态变量的相互影响关系，所以经过特定的数学逻辑也就是卡尔曼滤波可以通过观测一个状态变量求出其它所有状态变量包括观测量对应的状态变量。