背景介绍：
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, j<k, 分别称X'(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)]';end
R = 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;
效果视频
|
