卡尔曼滤波预测与测量我们更应该相信谁？

本文承接白色巧克力亦唯心的博文卡尔曼滤波 -- 从推导到应用（二）写一些修正和理解总结

刚刚步入机器人定位导航领域，内有感性理解，如有纰缪请指正，仅供交流。非常感谢白色巧克力亦唯心的总结与分享。精彩的博文，巨大的阅读量，对相关领域的蓬勃发展，将来可以说一句居功至伟。

卡尔曼滤波 -- 从推导到应用（一）公式推导细致入微。

卡尔曼滤波 -- 从推导到应用（二）例子承接（一），分析的很好，但有如下问题。

1、真实值并不是x，x是预测值。

真实值应该是：

xr=1/2*g*t.^2+Qn(int32(t/delta_t+1));%真实位置。其中数组Qn是每一个delta_t 的累计误差。每一次delta_t的噪声服从正态分布，均值为p，方差为q。

2、测量更新里测量的结果是真实值+测量噪声所以应该是：

z = xr +s+ r.*randn(1,N); % 测量时加入测量噪声。噪声R服从均值s,方差r的正态分布。

所以为了更真实地仿真滤波情景，代码修改后如下：

clc
clear all  ;
close all  ;%% 初始化参数
delta_t=0.1;   %采样时间
t=0:delta_t:10;
N = length(t); % 序列的长度
sz = [2,N];    % 信号需开辟的内存空间大小  2行*N列  2:为状态向量的维数n
g=10;          %加速度值
p = 0;         %期望u
q = 10;      %方差sigma
s = 2;        %期望
r = 5;        %方差sigma
Q =[q 0;0 0]; %假设建立的模型  噪声方差叠加在位移上 大小为n*n方阵 n=状态向量的维数
R = r;    % 位置测量方差估计，可以改变它来看不同效果  m*m      m=z(i)的维数
%%
A=[1 delta_t;0 1];  % n*n
B=[1/2*delta_t^2;delta_t];
H=[1,0];            % m*n  n=size(Q);  %n为一个1*2的向量  Q为方阵
m=size(R);  % 分配空间
P=zeros(n);           % 后验方差估计  n*n
P(1) = 3.6603;%%           构造系统误差数组Qn，下标n为每一时刻点的累计误差p+q*randn~Z(p,q)
Qn = zeros(1,N);
%%Statistics and Machine Learning Toolbox
Qn(1) = q*randn;
for i=2:1:NQn(i) = Qn(i-1) + q*randn;
end
%%x=1/2*g*t.^2;      %预测位置
xr=1/2*g*t.^2+Qn(int32(t/delta_t+1));%真实位置
%z = x + sqrt(10).*randn(1,N); % 测量时加入测量白噪声
z = xr +s+ r.*randn(1,N); % 测量时加入测量白噪声                                     %%xhat=zeros(sz);       % x的后验估计
xhatminus=zeros(sz);  % x的先验估计
Pminus=zeros(n);      % n*n
K=zeros(n(1),m(1));   % Kalman增益  n*m
I=eye(n);  %% 估计的初始值都为默认的0，即P=[0 0;0 0],xhat=0
for k = 9:N           %假设车子已经运动9个delta_T了，我们才开始估计  % 时间更新过程  xhatminus(:,k) = A*xhat(:,k-1)+B*g;  Pminus= A*P*A'+Q  % 测量更新过程  K = Pminus*H'*inv( H*Pminus*H'+R )  xhat(:,k) = xhatminus(:,k)+K*(z(k)-H*xhatminus(:,k));  P = (I-K*H)*Pminus
end
%%
figure(1)
plot(t,z);
hold on
plot(t,xhat(1,:),'r-')
plot(t,x(1,:),'k-');
hold on
plot(t,xr(1,:),'g-');
legend('含有噪声的测量', '后验估计', '独立预测值','真值');
xlabel('Iteration');

结果是这个样子的（每一次运行不一样，因为噪声是随机的）：

我们说卡尔曼滤波的问题实际上是预测和测量我们更相信谁的问题。这样说很好理解，但这是一个静态的理解。

实际上卡尔曼滤波是一个动态的过程。

预测，有误差分布Q

测量，有误差分布R

真值 =预测-Q

真值 =测量-R

无论是预测还是测量，每一步都是

预测（测量-R）+Q

测量（预测-Q）

一定要理解：

每一波测量都在前一步的预测和测量的基础上，而不是孤立的测量。

每一步的预测是基于前一步的预测和测量，而不是孤立的预测。如matlab曲线中的独立预测值曲线。

每一步的测量测量的是真实值也就是测的是上一步的预测-Q，所以测量不是孤立的。所以我们看到后验曲线在测量曲线周围而不在孤立的预测值曲线周围。

下面回答一下文章的标题。

数学角度：（公式摘自白色巧克力亦唯心卡尔曼滤波 -- 从推导到应用（一））

我们该相信谁？那么我们来求K吧。从贝叶斯估计的理念讲，权重，这一步就是表示这个权重，该相信谁多少。

怎么求K？求K使得估计值与真实值的最小均方差最小。

详细推导见白色巧克力亦唯心卡尔曼滤波 -- 从推导到应用（一）。

一开始要假定一个P的初值，相当于我们一开始先假定该相信谁多少。后面的每一步迭代实际上就是根据由两个噪声分布得到的数据去计算K。最终K，P，Pminus都收敛了（在本模型下，其它未知模型请自行验证，感悟）。

这个收敛的值就是我们应该相信谁的一个权重。下面结合上面的代码看一下输出值：

设置 P(1) =10 ：

Pminus =

20 0
0 0

K =

0.8000
0

P =

4.0000 0
0 0

Pminus =

14 0
0 0

K =

0.7368
0

P =

3.6842 0
0 0

Pminus =

13.6842 0
0 0

K =

0.7324
0

P =

3.6620 0
0 0

Pminus =

13.6620 0
0 0

K =

0.7321
0

P =

3.6604 0
0 0

Pminus =

13.6604 0
0 0

K =

0.7321
0

P =

3.6603 0
0 0

Pminus =

13.6603 0
0 0

K =

0.7321
0

P =

3.6603 0
0 0

一开始就设置p(1) =3.6603：

Pminus =

13.6603 0
0 0

K =

0.7321
0

P =

3.6603 0
0 0

Pminus =

13.6603 0
0 0

K =

0.7321
0

P =

3.6603 0
0 0

从一开始就把收敛值带入那么从一开始就收敛。

小结：

按照我的理解（刚入职，工作原因暂未查询资料验证，仅供交流），如果预测的噪声分布Q和测量的噪声分布R确定，那么该相信谁更多，K从一开始就是客观存在的，是确定的。而预测更新和测量更新，其实是一个通过迭代去得到这个确定的K值的过程。方法就是求K使得每次迭代的估计值与真实值的最小均方差最小。

那么既然该相信谁更多实际上是确定的，那么如果测量过程中出现高斯分布边缘的小概率的大误差卡尔曼滤波能否有效降低这个大的误差？

答案应该是否定的。我们最终将以同样的收敛的K的权重去相信两者，所以误差是与测量误差成线性关系的。

更相信谁与单个样本点无关，只与系统噪声和测量噪声的分布有关。

所以，卡尔曼滤波可以通过预测对测量进行优化，但是对于大的测量（如大的距离的匹配失准）是无能为力的。所以我们可以用卡尔曼滤波做优化，但是对于可能出现的大的误差或者说错误，需要想别的办法特殊处理。