ekf.m

高速车辆组合导航研究

% 我给你一个我编的程序。看不懂可以问我，它是
% 是一个目标匀速的一个模型，第一种方法是雷达只有测速功能。用的是标准卡尔曼滤波，第二种是目标带有测速（径向速度）功能，用的是非线性卡尔曼滤波。希望对你有帮助。
%此程序仿真了一个三坐标的跟踪系统，假设目标的高度不变
%在水平方向匀速飞行,x轴方向和y轴方向的速度分别为200m/s,150m/s,初始坐标为(100公里,50公里,1000米)
clear;
clc;
tic;
close all;
T=2;%雷达扫描周期为2秒
x0=10e4;%目标在x方向的初始位置为100公里
y0=5e4;%目标在y方向的初始位置为50公里
z0=1000;%目标在z方向的初始高度为1000米
v0x=200;
v0y=150;
t=0:T:400-T;%观测时间
x=x0+v0x*t;%目标在x轴上的真实运动轨迹
y=y0+v0y*t;%目标在y轴上的真实运动轨迹
z=z0+zeros(1,200);%目标在z轴上的真实运动轨迹
figure(13)
plot(x,y);zoom on;grid on;
title('目标在空间中的真实飞行轨迹')


deta_r=300;%观测距离的标准方差
deta_alpha=0.1*10^(-3);%观测方位角的标准方差
deta_beta=0.1*10^(-3);%观测仰角的标准方差,假设这3个都不变


k=1:200;
deta_x2(k)=deta_r^2*[x(k).^2/(x(k).^2+y(k).^2+z(k).^2)]+y(k).^2*deta_alpha^2+z(k).^2.*x(k).^2/(x(k).^2+y(k).^2)*deta_beta^2;
deta_y2(k)=deta_r^2*[y(k).^2/(x(k).^2+y(k).^2+z(k).^2)]+x(k).^2*deta_alpha^2+z(k).^2.*y(k).^2/(x(k).^2+y(k).^2)*deta_beta^2;
deta_z2(k)=deta_r^2*[z(k).^2/(x(k).^2+y(k).^2+z(k).^2)]+(x(k).^2+y(k).^2)*deta_beta^2;%这三个分别为在三个轴上的观测误差



noise=randn(1,200);
noise=[noise-mean(noise)]/sqrt(var(noise));%产生一个(0,1)分布的高斯白噪声
guance_x=x+sqrt(deta_x2).*noise;%模拟在x方向上的观测值
noise=randn(1,200);%不同的白噪声
noise=[noise-mean(noise)]/sqrt(var(noise));
guance_y=y+sqrt(deta_y2).*noise;%模拟在y方向上的观测值
noise=randn(1,200);
guance_z=z+sqrt(deta_z2).*noise;%模拟在z方向上的观测值
guance=zeros(3,1,200);
for k=1:200
guance(:,:,k)=[guance_x(k) guance_y(k) guance_z(k)]';
end
 figure(2)
 plot(guance_x);zoom on;grid on;
 xlabel('点数')
 ylabel('目标在x轴方向的距离观测值')
figure(3)
plot(guance_y);zoom on;grid on;
xlabel('点数')
ylabel('目标在y轴方向的距离观测值')
figure(4)
plot(guance_z);zoom on;grid on;
xlabel('点数')
ylabel('目标在z轴方向的距离观测值')
%初始化;
H=[1 0 0 0 0 0;0 1 0 0 0 0;0 0 1 0 0 0];%观测矩阵
D=[1 0 0 T 0 0;0 1 0 0 T 0;0 0 1 0 0 T;0 0 0 1 0 0;0 0 0 0 1 0;0 0 0 0 0 1];%一步状态转移矩阵
I=[1 0 0 0 0 0;0 1 0 0 0 0;0 0 1 0 0 0;0 0 0 1 0 0;0 0 0 0 1 0;0 0 0 0 0 1];%单位矩阵
R=zeros(3,3,200);
k=1:200;
deta_xy2(k)=(x(k).*y(k))/(x(k).^2+y(k).^2).*[deta_r^2*(x(k).^2+y(k).^2)/(x(k).^2+y(k).^2+z(k).^2)-(x(k).^2+y(k).^2)*deta_alpha^2+z(k).^2*deta_beta^2];
deta_xz2(k)=(x(k).*z(k))/(x(k).^2+y(k).^2+z(k).^2)*deta_r^2-x(k).*z(k)*deta_beta^2;
deta_yz2(k)=(y(k).*z(k))/(x(k).^2+y(k).^2+z(k).^2)*deta_r^2-y(k).*z(k)*deta_beta^2;
for k=1:200
R(:,:,k)=[deta_x2(k) deta_xy2(k) deta_xz2(k);deta_xy2(k) deta_y2(k) deta_yz2(k);deta_xz2(k) deta_yz2(k) deta_z2(k)];%观测误差的协方差矩阵
end
%对P0进行初始化
p11=deta_r^2*x(2)^2/(x(2)^2+y(2)^2+z(2)^2)+y(2)^2*deta_alpha^2+x(2)^2*z(2)^2/[x(2)^2+y(2)^2]*deta_beta^2;
p12=x(2)*y(2)/(x(2)^2+y(2)^2)*[deta_r^2*(x(2)^2+y(2)^2)/(x(2)^2+y(2)^2+z(2)^2)-(x(2)^2+y(2)^2)*deta_alpha^2+z(2)^2*deta_beta^2];
p13=x(2)*z(2)/(x(2)^2+y(2)^2+z(2)^2)*deta_r^2-x(2)*z(2)*deta_beta^2;
p14=p11/T;
p15=p12/T;
p16=p13/T;
p21=p12;
p22=deta_r^2*y(2)^2/(x(2)^2+y(2)^2+z(2)^2)+x(2)^2*deta_alpha^2+y(2)^2*z(2)^2/(x(2)^2+y(2)^2)*deta_beta^2;
p23=y(2)*z(2)/(x(2)^2+y(2)^2+z(2)^2)*deta_r^2-y(2)*z(2)*deta_beta^2;
p24=p12/T;
p25=p22/T;
p26=p23/T;
p31=p13;
p32=p23;
p33=deta_r^2*z(2)^2/(x(2)^2+y(2)^2+z(2)^2)+(x(2)^2+y(2)^2)*deta_beta^2;
p34=p13/T;
p35=p23/T;
p36=p33/T;
p41=p14;
p42=p24;
p43=p34;
p44=1/T^2*([deta_r^2*x(2)^2/(x(2)^2+y(2)^2+z(2)^2)+y(2)^2*deta_alpha^2+x(2)^2*z(2)^2/(x(2)^2+y(2)^2)]+[deta_r^2*x(1)^2/(x(1)^2+y(1)^2+z(1)^2)+y(1)^2*deta_alpha^2+x(1)^2*z(1)^2/(x(1)^2+y(1)^2)]);
p45=1/T^2*([deta_r^2*x(2)*y(2)/(x(2)^2+y(2)^2+z(2)^2)-x(2)*y(2)*deta_alpha^2+z(2)^2*x(2)*y(2)/(x(2)^2 +y(2)^2)*deta_beta^2] +[deta_r^2*x(1)*y(1)/(x(1)^2+y(1)^2+z(1)^2)-x(1)*y(1)*deta_alpha^2+z(1)^2 *x(1)*y(1)/(x(1)^2+y(1)^2)*deta_beta^2]);
p46=1/T^2*([x(2)*z(2)/(x(2)^2+y(2)^2+z(2)^2)*deta_r^2-x(2)*z(2)*deta_beta^2]+[x(1)*z(1)/(x(1)^2+y(1)^2+z(1)^2)*deta_r^2-x(1)*z(1)*deta_beta^2]);
p51=p15;
p52=p25;
p53=p35;
p54=p45;
p55=1/T^2*([deta_r^2*y(2)^2/(x(2)^2+y(2)^2+z(2)^2)+x(2)^2*deta_alpha^2+y(2)^2*z(2)^2/(x(2)^2+y(2)^2)] +[deta_r^2*y(1)^2/(x(1)^2+y(1)^2+z(1)^2)+x(1)^2*deta_alpha^2+y(1)^2*z(1)^2/(x(1)^2+y(1)^2)]);
p56=1/T^2*([y(2)*z(2)/(x(2)^2+y(2)^2+z(2)^2)*deta_r^2-y(2)*z(2)*deta_beta^2]+[y(1)*z(1)/(x(1)^2+y(1)^2+z(1)^2)*deta_r^2-y(1)*z(1)*deta_beta^2]);
p61=p16;
p62=p26;
p63=p36;
p64=p46;
p65=p56;
p66=1/T^2*([deta_r^2*z(2)^2/(x(2)^2+y(2)^2+z(2)^2)+(x(2)^2+y(2)^2)*deta_beta^2]+[deta_r^2*z(1)^2/(x(1)^2 +y(1)^2+z(1)^2)+(x(1)^2+y(1)^2)*deta_beta^2]);
P0=[p11 p12 p13 p14 p15 p16;p21 p22 p23 p24 p25 p26;p31 p32 p33 p34 p35 p36;p41 p42 p43 p44 p45 p46;p51 p52 p53 p54 p55 p56;p61 p62 p63 p64 p65 p66];
P=zeros(6,6,200);
P(:,:,2)=P0;
PP=zeros(6,6,200);
X=zeros(6,1,200);
XX=zeros(6,1,200);
K=zeros(6,3,200);
X0=[x(2) y(2) z(2) (x(2)-x(1))/T (y(2)-y(1))/T (z(2)-z(1))/T]';
X(:,:,2)=X0;
radius=3000;%跟踪波门的门限
%卡尔曼滤波
for k=2:199
PP(:,:,k+1)=D*P(:,:,k)*D';
K(:,:,k+1)=PP(:,:,k+1)*H'*[H*PP(:,:,k+1)*H'+R(:,:,k+1)]^(-1);
P(:,:,k+1)=[I-K(:,:,k+1)*H]*PP(:,:,k+1);
XX(:,:,k+1)=D*X(:,:,k);
m(k+1)=sqrt((guance(1,1,k+1)-XX(1,1,k+1))^2+(guance(2,1,k+1)-XX(2,1,k+1))^2+(guance(3,1,k+1)-XX(3,1,k+1))^2);
if m(k+1)>radius
X(:,:,k+1)=[guance_x(k+1) guance_y(k+1) guance_z(k+1) (guance_x(k+1)-guance_x(k))/T (guance_y(k+1)-guance_y(k))/T (guance_z(k+1)-guance_z(k))/T]';%如果预测值与观测值的门限大于radius,则滤波值用观测值来代替
else
X(:,:,k+1)=D*X(:,:,k)+K(:,:,k+1)*[guance(:,:,k+1)-H*D*X(:,:,k)];
end
end
x_filter=zeros(1,200);
y_filter=zeros(1,200);
z_filter=zeros(1,200);
k=1:200;
x_filter(k)=X(1,1,k);
y_filter(k)=X(2,1,k);
z_filter(k)=X(3,1,k);
x_filter(1)=guance_x(1);
x_filter(2)=guance_x(2);
y_filter(1)=guance_y(1);
y_filter(2)=guance_y(2);
z_filter(1)=guance_z(1);
z_filter(2)=guance_z(2);
figure(5)
plot3(guance_x,guance_y,guance_z);zoom on;grid on;
title('三维空间中的轨迹观测值')
figure(6)
plot(x_filter);zoom on;grid on;
xlabel('点数')
ylabel('经过卡尔曼滤波后的x轴方向的距离')
 figure(7)
 plot(y_filter);zoom on;grid on;
 xlabel('点数')
 ylabel('经过卡尔曼滤波后的y轴方向的距离')
figure(8)
plot(z_filter);zoom on;grid on;
xlabel('点数')
ylabel('经过卡尔曼滤波后的z轴方向的距离')
figure(9)
plot3(x_filter,y_filter,z_filter);zoom on;grid on;
title('经过卡尔曼滤波后的空间轨迹')
figure(10)
plot(x_filter,y_filter);zoom on;grid on;
title('经过滤波后目标在x-y平面上的运动轨迹')
figure(11)
plot(guance_x,guance_y);zoom on;grid on;
title('目标在x-y平面上的观测轨迹')
vx_filter1=zeros(1,200);
for k=1:200
vx_filter1(k)=X(4,1,k);
vy_filter1(k)=X(5,1,k);
vz_filter1(k)=X(6,1,k);
end
figure(12)
plot(vx_filter1);zoom on;grid on;
xlabel('点数')
ylabel('速度')
title('x方向的速度滤波值')
figure(13)
plot(vy_filter1);zoom on;grid on;
title('y方向的速度滤波值')
xlabel('点数')
ylabel('速度')
figure(14)
plot(vz_filter1);zoom on;grid on;
xlabel('点数')
ylabel('速度')
title('z方向的速度滤波值')
error_x1=zeros(1,200);
error_y1=zeros(1,200);
error_z1=zeros(1,200);
error_vx1=zeros(1,200);
error_vy1=zeros(1,200);
error_vz1=zeros(1,200);
k=1:200;
error_x1(k)=P(1,1,k);
error_y1(k)=P(2,2,k);
error_z1(k)=P(3,3,k);
error_vx1(k)=P(4,4,k);
error_vy1(k)=P(5,5,k);
error_vz1(k)=P(6,6,k);
figure(15)
plot(error_x1);zoom on;grid on;
xlabel('点数')
ylabel('误差')
title('位置x分量的滤波误差')
figure(16)
plot(error_y1);zoom on;grid on;
xlabel('点数')
ylabel('误差')
title('位置y分量的滤波误差')
figure(17)
plot(error_z1);zoom on;grid on;
xlabel('点数')
ylabel('误差')
title('位置z分量的滤波误差')
figure(18)
plot(error_vx1);zoom on;grid on;
xlabel('点数')
ylabel('误差')
title('速度x分量的滤波误差')
figure(19)
plot(error_vy1);zoom on;grid on;
xlabel('点数')
ylabel('误差')
title('速度y分量的滤波误差')
figure(20)
plot(error_vz1);zoom on;grid on;
xlabel('点数')
ylabel('误差')
title('速度z分量的滤波误差')
time=toc