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 = ';[*]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) = ;[*]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();[*][*]Fe=;[*]Fn=Fe;[*]F=;[*][*]H = ;[*]%计算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=;[*]Qn=Qe;[*]Q=; % 模型噪声协方差阵[*]%R = diag();% gps观测噪声协方差阵[*]R = diag();[*][*]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=;[*]Fn1=Fe1;[*]F1=;[*]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)-);[*] 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复制代码仿真结果：http://www.ilovematlab.cn/data/attachment/forum/201304/22/154239tvupjjwmtzuvj8qj.jpg http://www.ilovematlab.cn/data/attachment/forum/201304/22/154334u8977paavjhca0y1.jpg 看到没？滤波结果不正常了。我想大概是因为仿真设置的一些参数不合理导致的。即这些常数值：加速度时间相关常数：tao加速度噪声方差：Q初始状态和残差协方差：x0,p0观测向量协方差：R我的最终的疑惑：1，仿真时这些值如何确定，才能弄出一个比较理想的滤波效果？2，如果是实际的gps和DR数据，如何确定这些值？下面程序是修改后的：function gps_simuclc;clear;hits = 300;T=1;x0 = ';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) = ';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();Fe=;Fn=Fe;F=;H = ;%计算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=;Qn=Qe;Q=;% 模型噪声协方差阵R = diag();% gps观测噪声协方差阵R = diag();= 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 =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=;Fn1=Fe1;F1=;numstate = size(x0,1);Xk=zeros(6,hits);Xk_predict=zeros(6,hits);Pk_predict=zeros(6,6,hits);Pk=zeros(6,6,hits);Xk(:,1)=x0;Pk(:,:,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)-); Pk(:,:,t)=(eye(numstate)-Kk*H)*Pk_predict(:,:,t);end% fclose(fid);% x=Xk; 东西很新！
页:
[1]