1.什么是卡尔曼滤波

先举个例子说一下什么是卡尔曼滤波。

假如有一个小机器人可以在草地上自由的运动。为了让它实现导航，机器人需要知道自己所处的位置。那么这个小机器人如何才能知道自己在什么位置呢？有两种方式：

根据起始位置及自身的运动进行运动学计算，从而得到自身的位置信息。
根据自身携带的传感器测量自身的位置信息，如GPS。

那么，现在就存在一个问题。不论是根据运动学计算还是利用传感器信息，得到的位置信息都不可避免的存在误差。那机器人到底该相信哪个数据呢？有或者，如何根据这两种数据来确定自身的位置呢？最简单的方式就是取平均值，但是这种方式合理吗？凭什么每一项的权重都是0.5？显然这种方式不合适。

既然，取均值的方式不合理，那又该怎么做呢？卡尔曼滤波解决的就是这个问题！

卡尔曼滤波的本质就是根据”测量值”（如GPS数据）和 “预测量”（如运动学计算结果）及 “误差”，计算得到当前状态的最优量。

2.卡尔曼滤波的数学推导

2.1 状态方程和测量方程

首先假设我们知道一个线性系统的状态差分方程为 $x_k = Ax_{k-1}+Bu_{k-1}+w_{k-1} \tag{1}$ 其中 $x$ 是系统的状态向量，大小为n*1列。 $A$ 为转换矩阵，大小为 $n*n$ 。 $u$ 为系统输入，大小为 $k*1$ 。 $B$ 是将输入转换为状态的矩阵，大小为 $n*k$ 。随机变量 $w$ 为系统噪声。

这里说句题外话，什么是状态方程？说的通俗一点，就是预测方程。根据上一时刻的状态及控制量来预测当前时刻的状态，当然，这个过程中不可避免的存在误差。

系统的测量方程为 $z_k=Hx_k+v_k\tag{2}$ $z$ 是测量值，大小为 $m*1$ ， $H$ 也是状态变量到测量的转换矩阵。大小为 $m*n$ 。随机变量 $v$ 是测量噪声。

那什么又是测量方程呢？就是根据此刻的状态量及噪声得到的测量值的过程。另一方面测量方程也表明，测量值是由我们的状态值确定的。

对于状态方程中的系统噪声 $w$ 和测量噪声 $v$ ，假设服从如下多元高斯分布，并且 $w$ , $v$ 是相互独立的。其中 $Q$ , $R$ 为噪声变量的协方差矩阵。
在这里插入图片描述

2.2 卡尔曼滤波过程

在此，我们设 $\hat{x_k}'$ 为预测值（根据上一时刻的状态及控制量由状态方程得到的当前时刻的状态）、 $\hat{x_k}$ 为估计值（即卡尔曼滤波的最终的结果）、 $\hat{z_k}$ 是测量值的预测（即，根据预测的状态值 $\hat{x_k}'$ 得到的测量值）。则三者的关系为 $\hat{x_k} = \hat{x_k}'+K(z_k-\hat{z_k})=\hat{x_k}'+K(z_k-H\hat{x_k}') \tag{3}$

其中， $(z_k-H\hat{x_k}')$ 称之为残差，也就是预测的和你实际测量值之间的差距。如果这项等于0，说明预测和测量出的完全吻合。

这个公式说明什么呢？他的意思就是最终的估计值等于预测值加上乘以系数 $K$ 的测量残差。

从这个公式可以看出，我们可以根据状态方程得到预测值，根据测量方程可以得到测量值的预测，然后在根据公式3就可以得到最终的估计值。还有一个问题就是系数 $K$ 的确定。

××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××

接下来说一下参数 $K$ 的确定。

首先来看一下真实值与估计值之间协方差
$P_K = E(e_ke_k^T) = E[(x_k-\hat{x_k})(x_k-\hat{x_k})^T] \tag{4}$
将公式3中的估计值带入可得
$P_K = E[(x_k-\hat{x_k}'-K(z_k-\hat{z_k}))(x_k-\hat{x_k}'-K(z_k-\hat{z_k}))^T] \\ = E[(x_k-\hat{x_k}'-K(Hx_k+v_k-H\hat{x_k}'))(x_k-\hat{x_k}'-K(Hx_k+v_k-H\hat{x_k}'))^T] \\ = E[((I-KH)x_k-\hat{x_k}'-Kv_k+KH\hat{x_k}')((I-KH)x_k-\hat{x_k}'-Kv_k+KH\hat{x_k}')^T] \\ = E[((I-KH)(x_k-\hat{x_k}')-Kv_k)((I-KH)(x_k-\hat{x_k}')-Kv_k)^T] \tag{5}$

同理，预测值与真实值之间的写方差矩阵为
$P_K' = E[e_k'e_k'^T] = E[(x_k-\hat{x_k}')(x_k-\hat{x_k}')^T] \\=E[(A(x_{k-1}-\hat{x_{k-1}})+w_{k-1})(A(x_{k-1}-\hat{x_{k-1}})+w_{k-1})^T] \\=E[(Ae_{k-1})(Ae_{k-1})^T] +E[w_{k-1}w_{k-1}^T] = AP_{k-1}A^T+Q\tag{6}$
系统状态x变量和测量噪声之间是相互独立的。所以，公式5可以写为
$P_K = E[((I-KH)(x_k-\hat{x_k}')-Kv_k)((I-KH)(x_k-\hat{x_k}')-Kv_k)^T] \\ =(I-KH)E[(x_k-\hat{x_k}')(x_k-\hat{x_k}')^T](I-KH)^T+KE[v_kv_k^T]K^T \tag{7}$

将6式带入7式可得
$P_K =(I-KH)P_K'(I-KH)^T+KE[v_kv_k^T]K^T = P_K'-KHP_k'-P_K'H^TK^T +K(HP_k'H^T+R)K^T\tag{8}$

协方差矩阵的对角线元素就是方差。这样一来，把矩阵 $P-K$ 的对角线元素求和，用字母T来表示这种算子，他的学名叫矩阵的迹。则有
$T[P_K] = T[P_K']-T[KHP_k']-T[P_K'H^TK^T] +T[K(HP_k'H^T+R)K^T]\tag{9}$

这样，我们就得到了关于K的一个方程。对其求导可得：
在这里插入图片描述
令导数为0，可以求得：
$K =P_k'H^T (HP_k'H^T+R)^{-1}\tag{10}$

将K值带入公式8中，可得：
$P_K = (I-KH)P_k'\tag{11}$
×××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××

至此，推到过程结束。我们来理一下卡尔曼滤波的思路：

1.首先，根据公式1计算我们的预测值，根据公式计算预测值与真实值的协方差矩阵 $\hat{x_k}' = A\hat{x}_{k-1}+Bu_{k-1}$ $P_K' = AP_{k-1}A^T+Q$
2.根据1中得到的数据计算卡尔曼增益 $K$ $K =P_k'H^T (HP_k'H^T+R)^{-1}$
3.根据预测值和测量值以及卡尔曼增益来计算最终的结果 $\hat{x_k} =\hat{x_k}'+K(z_k-H\hat{x_k}')$
4.最后还要计算估计值和真实值之间的误差协方差矩阵，为下次递推做准备。 $P_K = (I-KH)P_k'$

卡尔曼滤波过程就是不断利用以上5个公式进行计算的过程。

3 卡尔曼滤波应用

此部分内容来自：https://blog.csdn.net/m0_38089090/article/details/79523784

应用背景是匀加速小车，该线性系统的状态差分方程为 $x_k = Ax_{k-1}+Bu_{k-1}+w_{k-1}$
对小车进行建模，ft为合力，小车的状态方程表示为
在这里插入图片描述
矩阵形式表示为

#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <eigen3/Eigen/Dense>//包含Eigen矩阵运算库，用于矩阵计算
#include <cmath>
#include <limits>//用于生成随机分布数列

using namespace std;
using Eigen::MatrixXd;

double generateGaussianNoise(double mu, double sigma);//随机高斯分布数列生成器函数

int main(int argc, char* argv[])
{
    //""中是txt文件路径，注意：路径要用//隔开
    ofstream fout("..//result.txt");



    const double delta_t = 0.1;//控制周期，100ms
    const int num = 100;//迭代次数
    const double acc = 10;//加速度，ft/m

    MatrixXd A(2,2);
    A(0,0) = 1;
    A(1,0) = 0;
    A(0,1) = delta_t;
    A(1,1) = 1;

    MatrixXd B(2,1);
    B(0,0) = pow(delta_t,2)/2;
    B(1,0) = delta_t;

    MatrixXd H(1,2);//测量的是小车的位移，速度为0
    H(0,0) = 1;
    H(0,1) = 0;

    MatrixXd Q(2,2);//过程激励噪声协方差，假设系统的噪声向量只存在速度分量上，且速度噪声的方差是一个常量0.01，位移分量上的系统噪声为0
    Q(0,0) = 0;
    Q(1,0) = 0;
    Q(0,1) = 0;
    Q(1,1) = 0.01;

    MatrixXd R(1,1);//观测噪声协方差，测量值只有位移，它的协方差矩阵大小是1*1，就是测量噪声的方差本身。
    R(0,0) = 10;

    //time初始化，产生时间序列
    vector<double> time(100, 0);
    for(decltype(time.size()) i = 0; i != num; ++i){
        time[i] = i * delta_t;
        //cout<<time[i]<<endl;
    }

    MatrixXd X_real(2,1);
    vector<MatrixXd> x_real, rand;
    //生成高斯分布的随机数
    for(int i = 0; i<100;++i){
        MatrixXd a(1,1);
        a(0,0) = generateGaussianNoise(0,sqrt(10));
        rand.push_back(a);
    }
    //生成真实的位移值
    for(int i = 0; i < num; ++i){
        X_real(0,0) = 0.5 * acc * pow(time[i],2);
        X_real(1,0) = 0;
        x_real.push_back(X_real);
    }

    //变量定义，包括状态预测值，状态估计值，测量值，预测状态与真实状态的协方差矩阵，估计状态和真实状态的协方差矩阵，初始值均为零
    MatrixXd X_evlt = MatrixXd::Constant(2,1,0), X_pdct = MatrixXd::Constant(2,1,0), Z_meas = MatrixXd::Constant(1,1,0),
            Pk = MatrixXd::Constant(2,2,0), Pk_p = MatrixXd::Constant(2,2,0), K = MatrixXd::Constant(2,1,0);

    vector<MatrixXd> x_evlt, x_pdct, z_meas, pk, pk_p, k;
    x_evlt.push_back(X_evlt);
    x_pdct.push_back(X_pdct);
    z_meas.push_back(Z_meas);
    pk.push_back(Pk);
    pk_p.push_back(Pk_p);
    k.push_back(K);

    //开始迭代
    for(int i = 1; i < num; ++i){
        //预测值
        X_pdct = A * x_evlt[i-1] + B * acc;
        x_pdct.push_back(X_pdct);
        //预测状态与真实状态的协方差矩阵，Pk'
        Pk_p = A * pk[i-1] * A.transpose() + Q;
        pk_p.push_back(Pk_p);
        //K:2x1
        MatrixXd tmp(1,1);
        tmp = H * pk_p[i] * H.transpose() + R;
        K = pk_p[i] * H.transpose() * tmp.inverse();
        k.push_back(K);
        //测量值z
        Z_meas = H * x_real[i] + rand[i];
        z_meas.push_back(Z_meas);
        //估计值
        X_evlt = x_pdct[i] + k[i] * (z_meas[i] - H * x_pdct[i]);
        x_evlt.push_back(X_evlt);
        //估计状态和真实状态的协方差矩阵，Pk
        Pk = (MatrixXd::Identity(2,2) - k[i] * H) * pk_p[i];
        pk.push_back(Pk);
    }

    cout<<"含噪声测量"<<"  "<<"后验估计"<<"  "<<"真值"<<"  "<<endl;

    for(int i = 0; i < num; ++i){
        cout<<z_meas[i]<<"  "<<x_evlt[i](0,0)<<"  "<<x_real[i](0,0)<<endl;
        fout<<z_meas[i]<<"  "<<x_evlt[i](0,0)<<"  "<<x_real[i](0,0)<<endl;//输出到txt文档，用于matlab绘图
        //cout<<k[i](1,0)<<endl;
        //fout<<rand[i](0,0)<<endl;
        //fout<<x_pdct[i](0,0)<<endl;
    }

    fout.close();

    return 0;
}

//生成高斯分布随机数的函数，网上找的
double generateGaussianNoise(double mu, double sigma)
{
    const double epsilon = std::numeric_limits<double>::min();
    const double two_pi = 2.0*3.14159265358979323846;

    static double z0, z1;
    static bool generate;
    generate = !generate;

    if (!generate)
        return z1 * sigma + mu;

    double u1, u2;
    do
    {
        u1 = rand() * (1.0 / RAND_MAX);
        u2 = rand() * (1.0 / RAND_MAX);
    }
    while ( u1 <= epsilon );

    z0 = sqrt(-2.0 * log(u1)) * cos(two_pi * u2);
    z1 = sqrt(-2.0 * log(u1)) * sin(two_pi * u2);
    return z0 * sigma + mu;
}

测试结果为：
在这里插入图片描述

我所理解的卡尔曼滤波——公式推导与应用