1 前言
最近学习了卡尔曼滤波，体会到了数据融合下进行最优估计的思想。如果你也是小白，可以通过这个例子自己动手感受数据融合。
学习资料参考B站大神DR_CAN博士，连接：【卡尔曼滤波器】直观理解与二维实例
2 案例
基于上述视频中Excel的例子，使用MATLAB编写了一个简单的卡尔曼滤波器，40行代码，简单易懂。
这是一个给匀速行走的人定位的例子，
假设人作匀速直线运动，根据匀速运动数学模型，就可以得到位置和速度信息（X）。但路上有各种因素，所以这个模型并非理想的，存在一定的误差W。
另外通过卫星（Global Positioning System，GPS）也可以得到人的位置和速度信息（Z），也存在一定的观测误差V。
其中，W和V相互独立，并且都满足均值为0的正态分布。
使用卡尔曼滤波的目的就是：根据上面两种不够精确的方法得到的数据给出一个比较准确的估计。
更新公式如下：
3 代码实现
代码如下：
close all
clear
clc
%%
Q = [1 0;0 1]; %过程误差，Q矩阵
R = [0.1 0;0 0.1]; %观测误差，R矩阵
A = [1 1;0 1]; %A矩阵
H = [1 0;0 1]; %H矩阵
I = [1 0;0 1]; %单位矩阵
epoch = 30;
P = [1 0;0 1];
X = ones(epoch, 2); %生成位置和速度矩阵
X(1,:) = [0 1]; %初始的实际位置和速度（从位置0开始，假定以匀速1运动）
Xa = ones(epoch, 2); %用a表示头上的小帽子，后验
Xa(1,:) = [0 1];
Xa_ = ones(epoch, 2); %先验
Xa_(1,:) = [0 1]; %初始化先验
Z = ones(epoch, 2); %测量的位置和速度
Z(1,:) = [0 1]; %初始测量的位置和速度
for k = 2:epoch
%更新误差
w1 = normrnd(0,sqrt(Q(1,1))); %位置过程误差
w2 = normrnd(0,sqrt(Q(2,2))); %速度过程误差
W = [w1 w2];
v1 = normrnd(0,sqrt(R(1,1))); %观测位置过程误差
v2 = normrnd(0,sqrt(R(2,2))); %观测速度过程误差
V = [v1 v2];
%更新实际位置、实际速度
X(k,:) = (A*X(k-1,:)'+ W')';
%更新测量位置、测量速度
Z(k,:) = (H*X(k,:)' + V')';
%预测
P_ = A*P*A' + Q;
Xa_(k,:) = (A*Xa(k-1,:)')'; %先验
%校正
K = P_*H'*inv((H*P_)*H' + R);
P = (I - K*H)*P_;
Xa(k,:) = (Xa_(k,:)' + K*(Z(k,:)' - H*Xa_(k,:)'))';
end
%通过以下三个值比较平均绝对误差。mae_Xa 小于mae_Xa_和mae_Z，说明融合有效
mae_Xa = sum(abs(Xa(:,1)-X(:,1)));
mae_Xa_ = sum(abs(Xa_(:,1)-X(:,1)));
mae_Z = sum(abs(Z(:,1)-X(:,1)));
plot(1:epoch, X(:,1), 'r*-', 1:epoch, Z(:,1), '-b*', 1:epoch, Xa_(:,1), '-g*', 1:epoch, Xa(:,1), '-k*','LineWidth',2)
hold on
xlabel step
ylabel 位置
legend('实际位置', '测量位置', '先验估计位置', '后验估计位置')结果分析
下面分析几种情况
4.1 过程误差标准差为1，测量误差标准差为0.1
这种情况下，可以想象男孩蒙着眼在家里运动，连自己都不清楚自己在哪个位置，但他老爸坐在沙发上看着他，说他在电视机旁。老爸是旁观者，并且睁着眼，所以观测误差很小。
这种场景下，老板的测量位置应该很接近实际位置。而男孩因为是蒙着眼运动，很难根据自己先前的估计位置继续估计自己现在的位置。
通过下图也可以看出，绿色的先验估计位置曲线偏离红色的实际位置，而蓝色的测量位置很接近实际位置。黑色曲线为后验估计位置，这个位置就是先验估计位置和测量位置数据融合得到的结果，也非常贴近实际位置。
4.2 过程误差标准差为0.1，测量误差标准差为1
这种情况下，测量位置准确率低，偏离实际位置较远。但需要注意的是，过程标准差小，不意味着先验估计位置就足够准确，先验估计位置由上一步的后验估计位置计算得到，包含了过程误差和测量误差。
文章就写到这，留一个问题：为什么卡尔曼估计结果（后验估计位置）并不总是最接近实际位置？