kalman滤波基础及matlab仿真_MPU6050 姿态解算系列四：线性 Kalman 滤波

背景介绍：
MPU6050 姿态解算系列目的是让初学者容易入门，所以表述上一直淡化有难度的数学语言而改用“文字语言的数学形式”。
Kalman 滤波涉及的数学内容比较多，网上有很多讲卡尔曼滤波原理的文章，数学功底欠佳的读者可能看不懂。Sugar 本篇的目标是：
用更通俗易懂的方式表达“线性 Kalman 滤波的效果”，让读者不需要太深的数学功底就能知道线性 Kalman 滤波的作用。

思维铺垫

Kalman 滤波用状态空间来描述研究对象，是一种时域下的滤波方法。在正式进入算法讲解之前，先罗列几个 Sugar 觉得比较重要的点来做预先的思维导向：
1、在 Kalman 滤波方法中，系统的观测噪声是状态估计所依赖的重要信息，并不是需要滤除的对象。
2、因为 Kalman 滤波是在时域下的递推形式，所以计算量较小，容易实现。
3、Kalman 滤波器可以看成：是状态变量在由观测生成的线性空间上的投影。

Kalman 滤波器与投影

“投影”英文是“projection”。

上面“思维铺垫”的第 3 点，我们可以这样写：

基于 k 个观测值对 j 时刻状态的估计 = proj( j 时刻系统的真实状态 | k 个观测值)

表示：j 时刻的状态估计是j 时刻真实状态在由某一角度上的 k 个观测值生成的线性空间上的投影。读着比较绕，下面 Sugar 举个实际的例子，在这个例子里，注意找到状态估计、真实状态，并理解什么是某一角度，例：

Sugar 在屋里写推文，听到阳台窗子的风声。
Sugar 想：“外面正在刮多大的风呢？”
在不打开窗子的情况下：Sugar 看窗外柳树的摆动幅度，
默默观察一会儿(默记了 k 个幅度值)，
估计现在有 3 级风力。

状态估计是现在有 3 级风力；真实状态是在刮某一级风，并不确切知道风力；某一角度是在屋里不开窗靠眼力估计风力的角度。
现在，我们可以把 Kalman 滤波的投影解释写的稍微数学一点：

X'(j|k) = proj( X(j) | Y(1), Y(2), ..., Y(k) )

X'(j|k) 表示 基于 k 个观测值对 j 时刻状态的估计；X(j) 表示 j 时刻的真实状态，显然这个是并不知道的；Y(1), Y(2), ..., Y(k) 表示 k 个观测生成的线性空间

延伸：对于 j=k, j>k, jX'(j|k)为 Kalman 滤波器、预报器和平滑器。滤波器一般是对当前状态噪声的处理。

状态描述

1、状态转移

A称作状态转移矩阵，这个公式的意思是：系统当前的状态能够通过用状态转移矩阵对上一时刻的状态进行转移，再加上系统当前的实际噪声来表示。
2、状态观测

z(t)表示对当前系统的观测值，这个公式是说：系统当前的观测值就是系统当前的状态与当前观测噪声v(t)之和 。C是观测矩阵，表示：对哪个目标进行了观测。
3、系统状态受额外影响的情况

(1) 什么叫额外影响
以*落体运动的观测为例，我们的目标观测量是“位置”和“速度”，而因为受到重力加速度的影响，我们目标观测量“位置”和“速度”都会受到影响。这里重力加速度g就叫额外影响。
(2) B*u(t)是什么
以位移和速度为例，第 1 点中的状态转移方程能描述“均速”运动，若速度是变化的，那么就要把速度变化对系统状态的影响考虑进来。但在状态转移矩阵 A 中只有“速度对位移的影响”，如果要引入加速度对速度和位移的影响，就要加上这个 B*u(t)。

上面引出的比较突然，Sugar 在 github 上准备了三个线性 Kalman 应用的 MATLAB 实时脚本用来解释上面的内容，在公众号回复 Kalman 获得。因为这里的内容很多，放推文里太占篇幅，所以就突然出现一下，点到为止了。

在做线性 Kalman 滤波的时候，因为我们并不知道系统的真实状态，所以我们会用系统当前的状态估计来代替系统当前的状态。既然不知道真实状态，自然也不可能知道系统当前的实际噪声w(t)，那么这个噪声该怎么办呢？答案是：忽略掉。因为噪声对系统的影响小，解决滤波问题才有意义。如果噪声已经大到足以覆盖系统状态的话，我们要考虑换个角度观测(就像在嘈杂的环境下打电话，如果听不清，最好的方法是换个清静的地方或者带上耳机，只是冲着电话大声喊对于听力是没有帮助的)。
说到这里，用于实际编程的公式就变成了：

线性 Kalman 滤波方程

在理解上面的内容后，我们只要记住下面 5 个方程就可以去设计线性 Kalman 滤波器了。

1、时间更新方程(2个)

Q是系统噪声方差，我们在第一个公式里忽略了系统噪声，这个系统噪声方差就变成了需要手调的值，其实际意义就是：给手动干预留下的一个入口。P是协方差，描述估计值的准确程度。在线性 Kalman 滤波里P不是我们关心的重点，只要按公式计算就行了。
注意：B*u(t)是可选项，并不是一定要加上，加与不加要看是否有额外影响目标观测量的因素。

2、状态更新方程(3个)

R是观测的方差，这个我们可以通过观测值的统计数据获得。K是Kalman 增益，其作用就是调整滤波程度的大小。

从这个公式我们来重新理解一下“思维铺垫”里的第 1 个点：Kalman 滤波是在利用系统当前的观测噪声。

最后一步是协方差P的迭代。

线性 Kalman 滤波用于姿态解算

以姿态解算为例，一步一步按上面的规则设计一个线性 Kalman 滤波器。

一、确定目标量

姿态解算我们关注的是：横滚角、横滚角速度、俯仰角和俯仰角速度。把这些目标量装到 state_estimate 列向量里，如下：

state_estimate = [横滚角; 横滚角速度; 俯仰角; 俯仰角速度];

为了便于表示，我们把文字改成英文：

state_estimate = [phi; phi_rate; theta; theta_rate];

二、确定状态转移矩阵

A = [ 1, dt, 0,  0][ 0,  1, 0,  0][ 0,  0, 1, dt][ 0,  0, 0,  1]

为什么 A 长这样的？我们看下 MATLAB 怎么说：

>> A*state_estimate ans =      phi + dt*phi_rate              phi_rate theta + dt*theta_rate            theta_rate

这样容易看出：结果仍然符合我们第一步确定的目标量的排列方式。

三、确定要不要有 B*u(t)

在姿态解算中，我们的目标量是“横滚角”、“俯仰角”、“横滚角速度”和“俯仰角速度”，这些都可以通过 MPU6050 观测到，没有影响目标量的额外因素。因此，在本篇的姿态解算中不需要 B*u(t)。

四、系统噪声 Q

系统噪声 Q 是留出的手动调节参数，看下 Sugar 在 MATLAB 里是怎么把这个调节接口留出来的：

var_acc_init   = 1e-2;var_gyro_init  = 6.75e0;var_acc        = [var_acc_init, var_acc_init]';var_gyro       = [var_gyro_init, var_gyro_init]';Q = eye(4);Q(1,1) = Q(1,1) * var_acc(1);Q(2,2) = Q(2,2) * var_gyro(1);Q(3,3) = Q(3,3) * var_acc(2);Q(4,4) = Q(4,4) * var_gyro(2);

五、观测噪声 R

观测噪声要随采样计算，Sugar 在 MATLAB 里取的是每一时刻的前 10 个采样值的方差给观测噪声 R，MATLAB 的程序是这样的：

cnt = 10;if i>cnt    var_acc(:,i)  = [var(phi_acc(size(phi_acc,1)-cnt:end)), var(theta_acc(size(theta_acc,1)-cnt:end))]';    var_gyro(:,i) = [var(phi_rate_gyro(size(phi_rate_gyro,1)-cnt:end)), var(theta_rate_gyro(size(theta_rate_gyro,1)-cnt:end))]';else    var_acc(:,i)  = [var(phi_acc), var(theta_acc)]';    var_gyro(:,i) = [var(phi_rate_gyro), var(theta_rate_gyro)]';endR = eye(4);R(1,1) = R(1,1) * var_acc(1,i);R(2,2) = R(2,2) * var_gyro(1,i);R(3,3) = R(3,3) * var_acc(2,i);R(4,4) = R(4,4) * var_gyro(2,i);

六、应用线性 Kalam 的 5 个公式开始递推

这一步没太多要说的，直接用上面介绍过的 5 个公式就行了，如下：

state_estimate = A * state_estimate;P = A * P * A' + Q;K = P * C' * inv(R + C * P * C');state_estimate = state_estimate + K * (measurement - C * state_estimate);P = (eye(4) - K * C) * P;

效果视频

往期回顾

姿态解算入门系列(算法篇)至此完结。
能跟完系列推文并掌握的读者已经顺利入门，入门不意味着终点。
滤波是一门比较深的学问，像“扩展卡尔曼滤波(EKF)”、“无迹卡尔曼滤波(UKF)”都要有入门的基础才能看得下去。

1、《MPU6050 抄底解读》
2、《MPU6050 姿态解算系列一：加速度姿态解算》
3、《MPU6050 姿态解算系列二：陀螺仪姿态解算》
4、《MPU6050 姿态解算系列三：互补滤波》

PS

Sugar 姿态解算系列推文重在算法入门，所有算法都是通过 MPU6050 的原始数据在 MATLAB 上做的。在公众号回复 att 得到全部的 MATLAB 代码。  

Sugar 姿态解算系列实验的设计如下：
1、STM32 读取 MPU6050 数据；
2、STM32 用 mavlink 打包数据，通过串口传给 ESP8266 Wifi 数传；
3、ESP8266 将串口接收到的 mavlink 包以 UDP 形式传到 Wifi 上；
4、MATLAB 从 Wifi 接 UDP 包并解析出 mavlink 报文得到数据；
5、用 MATLAB 研究算法。

姿态解算系列是在第 5 步展开的，而关注 Sugar 比较久的读者会知道 Sugar 在前 4 步上花的精力更多。前 4 步是通用的实验方法，学会了可以自定义任何实验。有关 mavlink 协议相关内容可以在公众号后台回复 mavlink 获得。

后面 Sugar 会继续姿态解算系列，计划不会再深入算法，而是打算写一写怎么把已经在 MATLAB 上验证过的算法写到单片机上。

参考链接

卡尔曼滤波器
https://longaspire.github.io/blog/%E5%8D%A1%E5%B0%94%E6%9B%BC%E6%BB%A4%E6%B3%A2/

关注作者

欢迎扫码关注我的公众号MultiMCU EDU。

提示：在公众号“关于我”页面可加作者微信好友。

喜欢本文求点赞，有打赏我会更有动力。