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

程序员文章站 2022-04-16 20:36:42

...

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

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

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

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

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:N
    Qn(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，所以测量不是孤立的。所以我们看到后验曲线在测量曲线周围而不在孤立的预测值曲线周围。