卡尔曼滤波的简单实现(Matlab）

转自：https://www.jianshu.com/p/c512a2b82907

理解起来方便简单快捷 故 转过来 记录用

卡尔曼滤波的数学原理当然是它最美妙的部分，不过据说挺难看懂的，我也就知难而退了，毕竟在程序中实现好像不需要非常理解其数学原理。有两篇英文博客写得不错，比较生动形象地描述了这个滤波器的工作过程，建议阅读：

• Kalman Filter For Dummies
• How a Kalman filter works, in pictures

一、最后一点点数学

为了不让实现滤波器的代码显得突兀，这里还是留下几个最最不可或缺的方程。实在心急，可以直接跳过这一部分。 （要找OC代码的朋友请不要一并跳过第二部分，那里讲到了参数设置）

• 被观测的系统

首先我们需要用方程来描述被观测的系统，因为后面滤波器的方程要用到这里的一些参数。卡尔曼滤波器适用于线性系统，设该系统的状态方程和观测方程为：

x(k) = A · x(k-1) + B · u(k) + w(k) z(k) = H · x(k) + y(k)

x(k) —— k时刻系统的状态 u(k) —— 控制量 w(k) —— 符合高斯分布的过程噪声，其协方差在下文中为Q z(k) —— k时刻系统的观测值 y(k) —— 符合高斯分布的测量噪声，其协方差在下文中为R A、B、H —— 系统参数，多输入多输出时为矩阵，单输入单输出时就是几个常数

注意在后面滤波器的方程中我们将不会再直接面对两个噪声w(k)和y(k)，而是用到他们的协方差Q和R。至此，A、B、H、Q、R这几个参数都由被观测的系统本身和测量过程中的噪声确定了。

• 滤波器

工程中用到的卡尔曼滤波是一个迭代的过程，每得到一个新的观测值迭代一次，来回来去地更新两个东西：“系统状态”(x)和“误差协方差”(P)。由于每次迭代只用到上一次迭代的结果和新的测量值，这样滤波对计算资源的占用是很小的。

每一次迭代，两个步骤：预测和修正。预测是根据前一时刻迭代的结果，即x(k-1|k-1)和P(k-1|k-1)，来预测这一时刻的系统状态和误差协方差，得到x(k|k-1)和P(k|k-1)：

x(k|k-1) = A · x(k-1|k-1) + B · u(k) P(k|k-1) = A · P(k-1|k-1) · A^T + Q

(这里用到的A、B、H、Q、R就是从前面的状态/观测方程中拿来的) 然后计算卡尔曼增益K(k)，和这一次的实际测量结果z(k)一起，用于修正系统状态x(k|k-1)及误差协方差P(k|k-1)，得到最新的x(k|k)和P(k|k)：

K(k) = P(k|k-1) · H^T · (H · P(k|k-1) · H^T + R)^-1 x(k|k) = x(k|k-1) + K(k) · (z(k) - H · x(k|k-1)) P(k|k) = (I - K(k) · H) · P(k|k-1)

好了好了不会有新的公式了。至此这次迭代就算结束了，x(k|k)就是我们要的滤波后的值，它和P(k|k)将会作为x(k-1|k-1)和P(k-1|k-1)用在下一时刻的迭代里。别看现在公式还是一大堆，在我们所谓的“简单场景”下，系统参数一定下来，就能简化很多很多。

先看状态方程和观测方程。假设我们是在测温度、加速度或者记录蓝牙RSSI，此时控制量是没有的，即：

B · u(k) ≡ 0

另外，参数A和H也简单地取1。现在滤波器的预测方程简化为：

① x(k|k-1) = x(k-1|k-1) ② P(k|k-1) = P(k-1|k-1) + Q

同时修正方程变成：

③ K(k) = P(k|k-1) / (P(k|k-1) + R) ④ x(k|k) = x(k|k-1) + K(k) · (z(k) - x(k|k-1)) ⑤ P(k|k) = (1 - K(k)) · P(k|k-1)

①对②、③无影响，于是④可以写成：

x(k|k) = x(k-1|k-1) + K(k) · (z(k) - x(k-1|k-1))

在程序中，该式写起来更简单：

x = x + K * (新观测值 - x);

观察②，对这一时刻的预测值不就是上一时刻的修正值+Q嘛，不妨把它合并到上一次迭代中，即⑤改写成：

P(k+1|k) = (1 - K(k)) · P(k|k-1) + Q

这一时刻的P(k+1|k)，会作为下一时刻的P(k|k-1)，刚好是③需要的。于是整个滤波过程只用这三个式子来迭代即可：

K(k) = P(k|k-1) / (P(k|k-1) + R)
x(k|k) = x(k-1|k-1) + K(k) · (z(k) - x(k-1|k-1))
P(k+1|k) = (1 - K(k)) · P(k|k-1) + Q

二、Matlab实现 & 参数设置

经过前面的推导，我们发现在程序中只需要不断重复这一小段即可实现卡尔曼滤波：

while(新观测值)
{
    K = P / (P + R);
    x = x + K * (新观测值 - x);
    P = (1 - K) · P + Q;
}

没看前面推导的朋友们别担心。现在我们只从程序的角度来看，不管物理意义。只要定好参数、赋好初值，它就能好好迭代下去。我们发现这一堆字母中，x和P只需要赋初值，每次迭代会产生新值；K用不着赋初值；Q和R赋值以后在之后的迭代中也可以改。

好消息是，x和P的初值是可以随便设的，强大的卡尔曼滤波器马上就能抹除不合理之处。但需注意，P的初值不能为0，否则滤波器会认为已经没有误差了，不玩儿了，看下图——

只要P(0) ≠ 0，一切都好。

剩下的也就是Q和R了。他俩的物理意义是噪声的协方差，他们的值是需要我们试出来的。举个例子，我们仍用上图的数据——

减小R值以后，滤波效果就没有这么明显了。但这并不意味着R值越大越好，因为我们的数据也可能是这样：

右图中增大了R值以后，虽然滤波结果更平滑，但滤波器会变得不敏感，会有滞后。因此应该根据具体的使用场景收集到的数据来决定Q和R的取值。他俩的取值当然也可以是时变的，可以识别跳变，可以自适应……这些就是高端玩法了，这里不做讨论。

至此，所有准备工作就做完了。Matlab代码也很简单：

% kalman_filter.m

function X = kalman_filter(data,Q,R,x0,P0)
N = length(data);

K = zeros(N,1);
X = zeros(N,1);
P = zeros(N,1);

X(1) = x0;
P(1) = P0;

for i = 2:N
    K(i) = P(i-1) / (P(i-1) + R);
    X(i) = X(i-1) + K(i) * (data(i) - X(i-1));
    P(i) = P(i-1) - K(i) * P(i-1) + Q;
end

调用及作图：

% test.m

clear;

data = [-59,-60,-55,-56,-56,-56,-57,-57,-59,-61,-57,-57,-57,-57,-57,-58,-57,-61,-57,-61,-59,-61,-60,-60,-59,-57,-56,-58,-58,-57,-57,-57,-57,-57,-60,-58,-61,-60,-60,-60,-57,-57,-57,-57,-58,-57,-58,-57,-58,-57,-58,-61,-58,-61,-62,-61,-61,-57,-57,-57,-57,-58,-57,-58,-57,-58,-61,-61,-60,-60,-58,-57,-57,-57,-57,-58,-58,-58,-58,-60,-61,-60,-57,-57,-57,-57,-57,-58,-58,-57,-57,-57,-60,-57,-60,-60,-59,-60,-57,-56]';
result = kalman_filter(data,1e-6,4e-4,-60,1);
i = 1:length(data);
plot(i,result,'r',i,data,'b');

做平面定位的时候可能需要滤x轴和y轴的数据，加速度计还可能有z轴数据。Matlab里面可以很方便地把A、B、H、Q、R变成矩阵，一次性把几个数据同时滤了；但实质上当这些数据互相之间没什么影响时，同时滤和分开滤是一毛一样的，所以上面的代码都可以不改，几个轴的数据分别调用滤波函数就是了。

卡尔曼滤波(KF)的简单使用就介绍到这里了。接下来还可以去实时调整Q和R，可以加控制量u(k)，可以进一步实现扩展卡尔曼滤波(EKF)、无迹卡尔曼滤波(UKF)等各种算法来应对非线性、非高斯等各种情况。只是对于我来说，现在这样就暂时够用了。

作者：伦啊伦
链接：https://www.jianshu.com/p/c512a2b82907
來源：简书
著作权归作者所有。商业转载请联系作者获得授权，非商业转载请注明出处。