“ 卡尔曼滤波是经典的预测追踪算法，是结合线性系统动态方程的维纳滤波，其实质是线性最小均方差估计器，能够在系统存在噪声和干扰的情况下进行系统状态的最优估计，广泛使用在导航、制导、控制相关领域。”

卡尔曼滤波器（kalman）主要包含五个公式，如下图，其中左边两个公式是预测公式，右边的三个是更新公式。

x——状态量

A——状态转移矩阵。

u——控制增益，通常为0。

B——控制增益的系数矩阵（实际应用场景很少需要关注到这个，所以不必太理会它）。

P——误差协方差矩阵。

H——观测矩阵。

K——卡尔曼增益。

Q——过程噪声协方差矩阵。

R——测量噪声协方差矩阵。

z——测量值。

初学者如果只看以上五个公式，是不是感觉有点懵？没关系，本文以最简单的一维卡尔曼滤波作为例子，介绍卡尔曼滤波的计算过程（不包含详细数学原理推导），由浅入深，看完之后会让你觉得，也不过如此，没那么复杂。

01

卡尔曼滤波的优缺点

优点：

• 卡尔曼滤波不仅具有滤波平滑的作用，还巧妙融合了测量数据与预测数据，将误差限定在一定范围；

• 卡尔曼滤波实时对最新预测值与测量值进行处理，不需要存储过去的数据，占用内存少且计算简单，因此适用于实时快速计算与嵌入式系统。

缺点：

• 卡尔曼滤波会一定程度造成信号的滞后；

• 原始的卡尔曼滤波算法适用于线性系统，但是实际应用场景中很多情况是非线性系统，所以在原始卡尔曼滤波的基础上后来又发展了针对非线性系统的扩展卡尔曼滤波；

• 卡尔曼滤波需要提前知道信号的预测模型方程（状态转移方程）、测量噪声与过程噪声的先验统计信息，才能得到最优滤波，然而实际应用场景往往得不到或很难得到这些先验信息。

02

—

一维卡尔曼滤波原理

智能驾驶领域，经常需要测量车辆的物理宽度W，每个时刻测量得到的W必然存在噪声，所以需要对W进行滤波。下面以使用kalman滤波对W进行滤波作为例子，介绍kalman滤波的计算过程。

• 明确状态量

状态量也即要滤波的对象，本例中滤波对象只有车辆的物理宽度W，因此其k时刻状态量为：

• 明确状态转移方程

状态转移方程，也即根据一定的数据模型，使用上一时刻的状态量来预测当前时刻的状态量。理论上车辆的物理宽度不随时间而改变，因此当前时刻状态量的预测值就是上一时刻的状态量（也即状态转移矩阵A就是1）：

• 明确观测矩阵
  

观测矩阵H，也即从状态量到测量值的转换矩阵：

矩阵H取值，常见的有以下三种情况：

（1）如果Z与X的维度都是n*1且两者包含的量一一对应，那么H就是维度为n*n单位向量：

（2）如果Z维度为n*1，X维度为m*1，且Z、X是线性关系，那么H是维度为n*m的对角矩阵：

（3）Z与X不是线性关系，那么观测矩阵H不能直接得到，可使用雅可比矩阵来近似H矩阵（扩展卡尔曼滤波，在接下来的文章会详细介绍）。比如X是极坐标，Z是笛卡尔坐标，那么两者就不是线性关系：

那么H可以近似为：

在本例中，状态量和测量值都是物理车宽W，Z与X一一对应，因此H矩阵直接取1。

• 误差协方差矩阵P的初始化
  

误差协方差矩阵P是kalman滤波过程中用于表示估计值误差的矩阵，也即表示系统状态估计值的不确定程度，该矩阵在滤波过程中动态调整，使用测量残差对其进行动态更新。

矩阵P的维度由状态量X的维度决定，假如X的维度为m*1，那么P的维度就是m*m的对角矩阵。滤波开始之前，需要对矩阵P进行初始化，通常将对角值初始化为1或较小值：

在本例中，P为1*1的矩阵，将其初始值设置为0.01。

• Q矩阵和R矩阵初始化
  

（1）Q为过程噪声协方差矩阵，Q为对角阵，Q的维度由状态量X的维度决定：X维度为m*1，那么Q的维度为m*m。Q由测量过程中各种噪声叠加综合决定：叠加噪声的方差越大，则Q应设置为越小值（也即Q值越小，滤波越平滑越滞后）。

（2）R为测量噪声协方差矩阵，R也为对角阵，R的维度由测量量Z的维度决定：Z维度为n*1，那么R的维度为n*n。R由传感器自身决定，比如相机出厂时自身参数会包含其噪声方差：传感器自身的噪声方差越大，则R应设置为越大值（也即R值越大，滤波越平滑越滞后）。

实际应用场景中，通常固定R矩阵，动态调整Q矩阵。一般将Q的对角值初始化为较小值（比如0.001）：

在本例中，Q和R都是1*1矩阵，将Q设置为0.003，R设置为2.15。

• 物理车宽kalman滤波的五个公式
  

综上，根据本文开头处五个kalman滤波通用公式得到本例的五个公式，其中前两个为预测步骤，后三个为更新步骤：

在公式（1）中，kalman滤波刚开始时X(k-1)是没有值的，所以在最开始时通常先将X(k-1)初始化为当前时刻的测量值：

• matlab实现物理车宽滤波
  

代码中假设真实车宽为2米，使用高斯噪声模拟实际测量过程中的噪声，给2米叠加上高斯噪声，然后对叠加了高斯噪声的信号进行卡尔曼滤波。

clc;
clear;


t = 1:1:1000;
s = repelem(2, length(t));   %假设车宽为固定2米
Z = s + normrnd(0, 0.15, 1, length(t));    %原信号叠加均值0方差0.15的高斯噪声，模拟实际场景中测量得到的车宽


A = 1;  %状态转移矩阵就是1
B = 0;   %控制增益u的系数矩阵
u = 0;    %u为控制增益，通常为0
Q = 0.003; %过程噪声协方差，Q越小，输出曲线越平滑但滞后越严重；Q越大，输出曲线越接近原曲线
R = 2.15;  %测量噪声协方差，R越小，输出曲线越接近原曲线；R越大，输出曲线越平滑但滞后越严重
P_k_1 = 0.01;   %上一时刻的误差协方差矩阵
P_k = 0;  %本时刻的误差协方差矩阵
X = Z(1);   %卡尔曼滤波输出值,将其初始化为测量值


for i=1:1:length(t)
    Xp = A * X + B * u;           % X(k)p=X(k-1)
    P_k = P_k_1 + Q;              % P(k)=P(k-1) + Q
    K = P_k / (P_k + R);          % K(k)=P(k) / (P(k) + R) 
    X = Xp + K * (Z(i) - Xp);     % X(k)=X(k)p + K * (Z(k) - X(k)p)
    P_k_1 = (1 - K) * P_k;        % P(k-1)=(1 - K) * P(k)
    
    result(i) = X;                %保存滤波结果
    
end


figure,
plot(t, Z);
hold on;
plot(t, result, 'Linewidth', 1);
ylim([0 3]);
legend('带噪声的实际车宽', '卡尔曼滤波输出');

运行以上matlab代码，得到结果如下：

好了，本文就写到这里。以上只是本人对卡尔曼滤波的一些理解，如有不对或疑问，欢迎留言沟通交流。接下来会继续介绍多维卡尔曼滤波、扩展卡尔曼滤波，敬请期待！