kalman滤波在车载导航中的应用

我最近在仿真车载导航中的卡拉曼滤波算法，之前看了论坛上的这篇文章(代码是这本书中的仿真实现：《kalman滤波理论及其在导航系统中的应用》)，基本理解了卡尔曼滤波算法的流程。http://www.ilovematlab.cn/thread-69151-1-1.html现在我有一个问题：这篇文章里面仿真的实际行车路线是通过公式计算出来的，即先求状态x(k+1)=f*x(k)+sita，然后取里面的位置坐标和航向 角，里程计读数等。观测路线是在此基础上加入了噪声。最后把这些代入卡拉曼滤波方程。说白了，它实际上已经把实际的观测向量预设成了满足卡拉曼滤波的条 件，即模型噪声和观测噪声都是正态高斯白噪声。但是如果实际的观测向量不是通过这种方式得到的，会怎么样呢？我模拟了一个这样的例子：实际的行车加速度是 随机的正态分布，然后通过牛顿力学运动公式求一个真实路线，再加入噪声构成观测向量，然后模拟，发现结果不正常。

• function gps_simu
• clc;
• clear;
• hits = 300;
• T=1;
• x0 = [100,10,0.001,100,10,0.001]';
• r=0.1;
• sim_acc_e = CreateGauss(0,r,1,hits);
• sim_acc_n = CreateGauss(0,r,1,hits);
• Z = zeros(2,hits);
• Z_view = zeros(2,hits);
• Z(:,1) = [x0(1) x0(4)];
• v01 = x0(2);v02 = x0(5);
• for t=1:T:hits-1
•     %Z(1,t+1) = Z(1,t)+10;
•     %Z(2,t+1) = Z(2,t)+10;
•     Z(1,t+1) = Z(1,t)+v01*T+0.5*sim_acc_e(t)*T^2;
•     Z(2,t+1) = Z(2,t)+v02*T+0.5*sim_acc_n(t)*T^2;
•     v01 = v01 + sim_acc_e(t)*T;
•     v02 = v02 + sim_acc_n(t)*T;
• end
• %加入噪声,模拟观测数据
• viewnoise_e = 15;
• viewnoise_n = 15;
• view_noise_e = CreateGauss(0,viewnoise_e^2,1,hits);
• view_noise_n = CreateGauss(0,viewnoise_n^2,1,hits);
• Z_view(1, = Z(1,+view_noise_e;
• Z_view(2, = Z(2,+view_noise_n;
• ae=1;an=1;
• zero_three = zeros(3,3);
• ae_Q=std(sim_acc_e);an_Q=std(sim_acc_n);%加速度过程噪声方差
• p0 = diag([15^2,0.01,ae_Q^2,15^2,0.01,an_Q^2]);
• Fe=[1 T ae^(-2)*(-1+ae*T+exp(-ae*T));0 1 ae^(-1)*(1-exp(-ae*T));0 0 exp(-ae*T)];
• Fn=Fe;
• F=[Fe zero_three;zero_three Fn];
• H = [1 0 0 0 0 0;0 0 0 1 0 0];
• %计算Q，R
• q11=0.5*ae^(-5)*(1-exp(-2*ae*T)+2*ae*T+2*ae^3*T^3*3^(-1)-2*ae^2*T^2-4*ae*T*exp(-ae*T));
• q12=0.5*ae^(-4)*(1+exp(-2*ae*T)-2*exp(-ae*T)-2*ae*T+ae^2*T^2+2*ae*T*exp(-ae*T));
• q21=q12;
• q13=0.5*ae^(-3)*(1-exp(-2*ae*T)-2*ae*T*exp(-ae*T));
• q31=q13;
• q22=0.5*ae^(-3)*(-3-exp(-2*ae*T)+4*exp(-ae*T)+2*ae*T);
• q23=0.5*ae^(-2)*(1+exp(-2*ae*T)-2*exp(-ae*T));
• q32=q23;
• q33=0.5*ae^(-1)*(1-exp(-2*ae*T));
• Qe=[q11 q12 q13;q21 q22 q23;q31 q32 q33];
• Qn=Qe;
• Q=[2*ae_Q^2*ae*Qe,zero_three;zero_three,2*an_Q^2*an*Qn]; % 模型噪声协方差阵
• %R = diag([viewnoise_e,viewnoise_n]);% gps观测噪声协方差阵
• R = diag([1,1]);
• x_filter = kalman(x0,p0,Z_view,F,H,Q,R,T,hits);
• figure(1)
• plot(Z(1,,Z(2,,'-r');
• figure(2)
• t=1:T:hits;
• plot(t,Z(1,-Z_view(1,,'-r');
• hold on;
• plot(t,Z(1,-x_filter(1,,'-g');
• hold off;
• legend('滤波之前','滤波之后');
• function x=kalman(x0,p0,Z,F,H,Q,R,T,hits)
• fid = fopen('for_test1.txt','wt');
• if fid==0
•     error('打开文件错误!');
•     return;
• end
• zero_three = zeros(3,3);
• Fe1=[1 T (T^2)/2;0 1 T;0 0 1];
• Fn1=Fe1;
• F1=[Fe1 zero_three;zero_three Fn1];
• numstate = size(x0,1);
• for t=1:T:hits
•     %%gps滤波
•    
•     %时间更新方程
•     Xk_predict = F1*x0;%状态x0不变
•     Pk_predict = F*p0*F'+Q;
•    
• for i=1:6
•      for j=1:6
•          fprintf(fid,'%16.8f        ',p0(i,j));%%%test,记录P
•      end
•      fprintf(fid,'
  ');
•     end
•     fprintf(fid,'
  ');
•    
•     %状态校正方程
•     Kk=Pk_predict*H'*inv(H*Pk_predict*H'+R);
•     Xk(:,t)=Xk_predict+Kk*(Z(:,t)-[Xk_predict(1);Xk_predict(4)]);
•     Pk=(eye(numstate)-Kk*H)*Pk_predict;
•    
•     X0 = Xk(:,t);
•     P0 = Pk;
• end
• fclose(fid);
• x=Xk;

复制代码产生高斯分布随机向量的函数CreateGauss为：

• function G=CreateGauss(E,D,M,N)
• %产生均值为E,方差为D,MxN的高斯白噪声矩阵
• %
•     G=randn(M,N);
•     G=G/std(G);
•     G=G-mean(G);
•     a=E;
•     b=sqrt(D);
•     G=a+b*G;
• end

复制代码仿真结果：

                               
登录/注册后可看大图

      

                               
登录/注册后可看大图

看到没？滤波结果不正常了。我想大概是因为仿真设置的一些参数不合理导致的。即这些常数值：加速度时间相关常数：tao加速度噪声方差：Q初始状态和残差协方差：x0,p0观测向量协方差：R我的最终的疑惑：1，仿真时这些值如何确定，才能弄出一个比较理想的滤波效果？2，如果是实际的gps和DR数据，如何确定这些值？下面程序是修改后的：function gps_simuclc;clear;hits = 300;T=1;x0 = [100,10,0.001,100,10,0.001]';r=0.1;normrnd('seed',18);sim_acc_e = normrnd(0,r,1,hits);sim_acc_n = normrnd(0,r,1,hits);Z = zeros(2,hits);Z_view = zeros(2,hits);Z(:,1) = [x0(1) x0(4)]';v01 = x0(2);v02 = x0(5);for t=1:T:hits-1    %Z(1,t+1) = Z(1,t)+10;    %Z(2,t+1) = Z(2,t)+10;    Z(1,t+1) = Z(1,t)+v01*T+0.5*sim_acc_e(t)*T^2;    Z(2,t+1) = Z(2,t)+v02*T+0.5*sim_acc_n(t)*T^2;    v01 = v01 + sim_acc_e(t)*T;    v02 = v02 + sim_acc_n(t)*T;%%%%%%真实end%加入噪声,模拟观测数据viewnoise_e = 5;viewnoise_n = 5;view_noise_e = normrnd(0,viewnoise_e^2,1,hits);view_noise_n = normrnd(0,viewnoise_n^2,1,hits);Z_view(1,:) = Z(1,:)+view_noise_e;Z_view(2,:) = Z(2,:)+view_noise_n;ae=1;an=1;zero_three = zeros(3,3);ae_Q=std(sim_acc_e);an_Q=std(sim_acc_n);%加速度过程噪声方差p0 = diag([15^2,0.01,ae_Q^2,15^2,0.01,an_Q^2]);Fe=[1 T ae^(-2)*(-1+ae*T+exp(-ae*T));0 1 ae^(-1)*(1-exp(-ae*T));0 0 exp(-ae*T)];Fn=Fe;F=[Fe zero_three;zero_three Fn];H = [1 0 0 0 0 0;0 0 0 1 0 0];%计算Q，Rq11=0.5*ae^(-5)*(1-exp(-2*ae*T)+2*ae*T+2*ae^3*T^3*3^(-1)-2*ae^2*T^2-4*ae*T*exp(-ae*T));q12=0.5*ae^(-4)*(1+exp(-2*ae*T)-2*exp(-ae*T)-2*ae*T+ae^2*T^2+2*ae*T*exp(-ae*T));q21=q12;q13=0.5*ae^(-3)*(1-exp(-2*ae*T)-2*ae*T*exp(-ae*T));q31=q13;q22=0.5*ae^(-3)*(-3-exp(-2*ae*T)+4*exp(-ae*T)+2*ae*T);q23=0.5*ae^(-2)*(1+exp(-2*ae*T)-2*exp(-ae*T));q32=q23;q33=0.5*ae^(-1)*(1-exp(-2*ae*T));Qe=[q11 q12 q13;q21 q22 q23;q31 q32 q33];Qn=Qe;Q=[2*ae_Q^2*ae*Qe,zero_three;zero_three,2*an_Q^2*an*Qn];% 模型噪声协方差阵R = diag([viewnoise_e,viewnoise_n]);% gps观测噪声协方差阵R = diag([1,1]);[Xk_predict,Xk,Pk]= kalman(x0,p0,Z_view,F,H,Q,R,T,hits);figure(1)plot(Z(1,:),Z(2,:),'-r');%%%%%%figure(2)t=1:T:hits;plot(t,Z(1,:)-Z_view(1,:),'-r');hold on;plot(t,Z(1,:)-Xk(1,:),'-g');hold off;legend('滤波之前','滤波之后');%%%%%%%%%%figure(3)t=1:T:hits;plot(Z_view(1,:),Z_view(2,:),'-r');hold on;plot(Xk_predict(1,2:end),Xk_predict(4,2:end),'-b');hold onplot(Xk(1,:),Xk(4,:),'-g');hold offlegend('滤波之前','滤波预测','滤波之后');function [Xk_predict,Xk,Pk]=kalman(x0,p0,Z,F,H,Q,R,T,hits)% fid = fopen('for_test1.txt','wt');% if fid==0%   error('打开文件错误!');%   return;% endzero_three = zeros(3,3);Fe1=[1 T (T^2)/2;0 1 T;0 0 1];Fn1=Fe1;F1=[Fe1 zero_three;zero_three Fn1];numstate = size(x0,1);Xk=zeros(6,hits);Xk_predict=zeros(6,hits)k_predict=zeros(6,6,hits)k=zeros(6,6,hits);Xk(:,1)=x0k(:,:,1)=p0;for t=2:T:hits    %%gps滤波    %时间更新方程    Xk_predict(:,t) = F1*Xk(:,t-1);%状态x0不变    Pk_predict(:,:,t) = F*Pk(:,:,t-1)*F'+Q;% for i=1:6%       for j=1:6%          fprintf(fid,'%16.8f        ',p0(i,j));%%%test,记录P%       end%       fprintf(fid,'
');%   end%   fprintf(fid,'
');    %状态校正方程    Kk=Pk_predict(:,:,t)*H'*inv(H*Pk_predict(:,:,t)*H'+R);    Xk(:,t)=Xk_predict(:,t)+Kk*(Z(:,t)-[Xk_predict(1,t);Xk_predict(4,t);]);    Pk(:,:,t)=(eye(numstate)-Kk*H)*Pk_predict(:,:,t);end% fclose(fid);% x=Xk;

东西很新！