卡尔曼滤波器原理及代码实现

卡尔曼滤波（Kalman filtering）它是一种利用线性系统状态方程通过系统输入输出观测数据对系统状态进行最优估计的算法。由于观测数据包括系统中噪声和干扰的影响，最优估计也可视为滤波过程。

也叫卡尔曼滤波器最佳线性滤波器，它有很多优点:简单，占用内存小，速度快。卡尔曼滤波器也是时域滤波器(不需要频域变换)。

用简单的例子介绍卡尔曼滤波器原理： 假设一辆车在路上行驶，通常是通过描述这辆车的状态来描述的位置和速度来表示： x t x_t xt表示当前状态， p t p_t pt代表当前位置， v t v_t vt代表当前的速度。同时，运动学公式描述了当前时刻和前一刻的状态： 以矩阵的形式表示： 假设两个变量(位置和速度)都服从卡尔曼滤波器高斯分布的随机变量。每个变量都有一个均值 μ，表示随机分布的中心（最有可能的状态）和方差表示变量的不确定性。在这种情况下，可以发现位置和速度是相关的。因为速度越大，理论上和下一刻的位置变化就越大。因此，使用这种相关性协方差矩阵来表示。 F t F_t Ft为了预测矩阵，我们可以更新上述运动学公式 x t x_t xt​的状态，但协方差矩阵应该如何更新呢？这里需要引入一个关于协方差的公式：

因此可以得到：

若在此基础上有外部因素对汽车进行控制(如踩油门)，我们还需要引入一个加速度a，因此，位置与速度的方程可表示为：

通过观察可以发现，当前时刻的位置和速度都是上一时刻位置和速度的线性组合，若用矩阵的形式来表示：

F t F_t Ft​为预测矩阵， B t B_t Bt​为控制矩阵。此时预测的状态 x t x_t xt​并非就是准确的，还需要考虑外部干扰，如摩擦力、阻力等。所以再每次预测之后，我们需要增加一些“不确定性”。

原始估计中的每个状态变量更新到新的状态后，仍然服从高斯分布。我们可以说 x t − 1 x_t-1 xt​−1的每个状态变量移动到了一个新的服从高斯分布的区域，协方差为 Q t Q_t Qt​。也就是将这些没有被跟踪的干扰当作协方差为 Q t Q_t Qt​的噪声来处理。得到新的预测公式：

用观测值来修正预测值： 假设使用一个传感器来测量汽车当前的状态，传感器读取的数据的单位和尺度有可能与我们要跟踪的状态的单位和尺度不一样，我们用矩阵 H t H_t Ht​来表示传感器的数据。同时由于观测到的数据也并非完全正确，因此同样增加一个噪声v。那么同样，观测值的协方差矩阵也相应要加上一个噪声R。

之前所说的预测公式也不能百分之百确定下一时刻的状态，所以要用观测值进行修正。这里涉及到卡尔曼增益，由于过程略复杂，在这里先跳过，日后再补充。这里直接给出修正后的状态方程： 其中卡尔曼增益系数 K ′ K' K′为：

该项为实际观测值与预期观测值之间的残差，卡尔曼增益可以调节模型依赖观测值多一些还是预测值多一些。

至此，我们得到了卡尔曼滤波器的所有五个方程

有了这五个方程，就可以在代码中应用。具体matlab实现代码如下：

Z=(1:100);
noise=randn(1,100);%随机生成一个1*100的噪声
Z=Z+noise;

X=[0;0];%初始状态均为0
P=[1 0;0 1];%初始化协方差矩阵
F=[1 1;0 1];%时间间隔Δt为1
Q=[0.0001 0;0 0.0001];
H=[1 0];%观测矩阵
R=1;

figure;
hold on;

for i=1:100
    X_=F*X;
    P_=F*P*F'+Q;
    K=P_*H'/(H*P_*H'+R);
    X=X_+K*(Z(i)-H*X_);
    P=(eye(2)-K*H)*P_;
    
    plot(X(1),X(2),'x');
end

这里将观测矩阵 H H H设置为[1 0], H H H与 x t x_t xt​相乘后得到汽车的位置。