研发埠

标题: kalman滤波在车载导航中的应用 [打印本页]

作者: 韦常柱 时间: 2015-1-12 11:11
标题: 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)=x0

k(:,:,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;

作者: 杨希祥 时间: 2015-1-14 13:36
东西很新！

欢迎光临研发埠 (http://bbs.yanfabu.com/)