ekf_trace.m

书籍《多传感器数据融合》P44页算法的具体实现

%参考文献
%多传感器数据融合及其应用（杨万海著） 
%P45-46页 
% D R的确定方法见P41页 2-1-102到104

clear all;
clc;

T=5; % 采样周期
N=40; % 采样点数

x0=20;       %目标在x方向的初始位置为20公里
y0=20;       %目标在y方向的初始位置为20公里
v0x= -0.2;  %目标在x方向的速度       
v0y=0;        %目标在y方向的速度
deta_r=250;    %观测距离误差的标准方差
deta_theta=0.0005; %观测方位角误差的标准方差

%装载观测数据
load trace.mat;
for k=1:N
xx1(k)=xx(k,1)/1000
xx2(k)=xx(k,2)/1000
end

Z_T=[xx1;xx2];

temp=zeros(2,40);

F=[1 0 T 0;0 1 0 T;0 0 1 0;0 0 0 1];

Q=[0.001 0 0 0;0 0.001 0 0;0 0 0.001 0;0 0 0 0.001];

I=[1 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1];
%计算观测噪声x,y的协方差矩阵R
R=[0.25 0; 0 0.0001];

%注意每次循环都要给X0和P0赋初值
X0=[x0,y0,v0x,v0y]';  %状态向量初值
P0=[1.0 0 0 0;0 1.0 0 0;0 0 0.01 0;0 0 0 0.01]; %估计误差协方差阵初值

for t=1:N-1
Xk_predict=F*X0;   %预测时，使用直角坐标系

%坐标转换，转化成极坐标
xm=Xk_predict(1);      
ym=Xk_predict(2);
Z_predict=[sqrt(xm^2+ym^2),atan(ym/xm)]';
%[THETA,PHI,R] = cart2sph(xm,ym,0);

%计算测量矩阵
r2=sqrt(xm^2+ym^2);
h1=[xm/r2, -ym/r2^2]';
h2=[ym/r2, xm/r2^2]';
O2=[0,0]';
H=[h1,h2,O2,O2];

%预测协方差阵
Pk_predict=F*P0*F'+Q;
S=H*Pk_predict*H'+R; %信息协方差阵
K=Pk_predict*H'*inv(S); %增益矩阵
temp(:,t)=Z_T(:,t)-Z_predict;
Xk(:,t)=Xk_predict+K*(Z_T(:,t)-Z_predict);  %估计矩阵(最后的输出值)

Pk=(eye(4)-K*H)*Pk_predict; %估计误差协方差阵
P0=Pk; %估计误差协方差阵
X0=Xk(:,t); %估计矩阵更新
end


for k=1:N-1
x_filter(k)=Xk(1,k);
y_filter(k)=Xk(2,k);
vx_filter(k)=Xk(3,k);
vy_filter(k)=Xk(4,k);
end

%直角坐标系到极坐标系的转换
[THETA,PHI,Ro] = cart2sph(x_filter,y_filter,0);

[traceX,traceY,traceZ] = sph2cart(xx2,0,xx1);

figure(1)
plot(traceX);zoom on;grid on;
hold on;
plot(x_filter,'r:');zoom on;grid on;
xlabel('点数')
ylabel('距离')
title('经过卡尔曼滤波以后的x方向上的距离')

% figure(2)
% plot(guance_y);zoom on;grid on;
% hold on;
% plot(y_filter2,'r:');zoom on;grid on;
% xlabel('点数')
% ylabel('距离')
% title('经过卡尔曼滤波以后的y方向上的距离')
% 
% figure(3)
% plot(Ro);zoom on;grid on;
% xlabel('点数')
% ylabel('距离')
% title('经过卡尔曼滤波以后的极坐标下的观测')
% 
% figure(4)
% plot(vx_filter1);zoom on;grid on;
% hold on;
% plot(vx_filter2,'r:');zoom on;grid on;
% xlabel('点数')
% ylabel('速度')
% title('x方向上的速度滤波值')
% 
% figure(5)
% plot(vy_filter1);zoom on;grid on;
% hold on;
% plot(vy_filter2,'r:');zoom on;grid on;
% xlabel('点数')
% ylabel('速度')
% title('y方向上的速度滤波值')