%% 导入数据
clc;clear;
data_m = importdata('Measure.mat');%
data_r = importdata('Real.mat');%
%% 参数初始化
N = size(data_m,2);%采样次数
T = 0.1;%采样时间
delta_w = 1;%状态噪声
v_r = 10^2;%方位角观测方差(m^2)
v_phi = (0.1 / 180 * pi)^2;%距离观测方差(rad^2);
R = [v_r/T 0;0 v_phi/T];
%% 模型建立
% CA模型
Phi = [1 T T^2/2 0 0 0;0 1 T 0 0 0;0 0 1 0 0 0;0 0 0 1 T T^2/2;0 0 0 0 1 T;0 0 0 0 0 1];%状态转移矩阵
Gamma = [T^2/2 0;T 0;1 0;0 T^2/2;0 T;0 1];
% Q = [T^4/4 T^3/2 T^2/2 0 0 0;
% T^3/2 T^2/2 T 0 0 0;
% T^2/2 T 1 0 0 0;
% 0 0 0 T^4/4 T^3/2 T^2/2;
% 0 0 0 T^3/2 T^2/2 T;
% 0 0 0 T^2/2 T 1];
Q = Gamma * Gamma';%系统噪声矩阵
%Q=eye(6)*10;
X = zeros(6,N);Z = zeros(2,N);X_ekf = zeros(6,N);
X(:,1) = [9900 0 0 0 195 -6]';%目标初始位置与速度
X_ekf(:,1) = X(:,1);Z(:,1) = [data_m(2,1);data_m(3,1)];
P_a = [100 0 0 0 0 0;0 10 0 0 0 0;0 0 5 0 0 0;...
0 0 0 100 0 0;0 0 0 0 10 0;0 0 0 0 0 5;];%协方差阵初始化
for i = 2:N
Z(:,i) = [data_m(2,i);data_m(3,i)];%观测数据
X_pre = Phi * X_ekf(:,i-1);%预测值
P_b = Phi * P_a * Phi' + Q * delta_w ^ 2;%更新预测协方差阵
H = [X_pre(1) / sqrt(X_pre(1).^2+X_pre(4).^2) 0 0 X_pre(4) / sqrt(X_pre(1).^2+X_pre(4).^2) 0 0;...
-X_pre(4) / (X_pre(1).^2+X_pre(4).^2) 0 0 X_pre(1) / (X_pre(1).^2+X_pre(4).^2) 0 0];%观测阵
K = P_b * H' * inv(H * P_b * H' + R);%增益阵
h_obv = [sqrt(X_pre(1).^2+X_pre(4).^2);atan(X_pre(4) / ((-1)^(X_pre(1) < 0) * X_pre(1)))];%观测值
X_ekf(:,i) = X_pre + K * (Z(:,i) - h_obv);%更新状态
P_a = (eye(6) - K * H) * P_b;%更新滤波协方差阵
end
figure
grid on;
plot(data_r(2,:),data_r(3,:),'r');hold on;
plot(X_ekf(1,:),X_ekf(4,:),'k');
legend('真值','EKF滤波值');
xlabel('X方向(m)');ylabel('Y方向(m)');title('目标运动轨迹真值与滤波值');
figure
subplot(1,2,1);grid on;
plot(0:0.1:50,data_r(4,:),'r');hold on;plot(0:0.1:50,X_ekf(2,:),'k');
legend('真值','EKF滤波值');
xlabel('时间(s)');ylabel('X方向速度(m/s)');title('X方向目标运动速度真值与滤波值');
subplot(1,2,2);grid on;
plot(0:0.1:50,data_r(5,:),'r');hold on;plot(0:0.1:50,X_ekf(5,:),'k');
legend('真值','EKF滤波值');
xlabel('时间(s)');ylabel('Y方向速度(m/s)');title('Y方向目标运动速度真值与滤波值');
figure
subplot(1,2,1);grid on;
plot(0:0.1:50,data_r(6,:),'r');hold on;plot(0:0.1:50,X_ekf(3,:),'k');
legend('真值','EKF滤波值');
xlabel('时间(s)');ylabel('X方向加速度(m/s^2)');title('X方向目标运动加速度真值与滤波值');
subplot(1,2,2);grid on;
plot(0:0.1:50,data_r(7,:),'r');hold on;plot(0:0.1:50,X_ekf(6,:),'k');
legend('真值','EKF滤波值');
xlabel('时间(s)');ylabel('Y方向加速度(m/s^2)');title('Y方向目标运动加速度真值与滤波值');
figure
subplot(1,2,1);grid on;
plot(0:0.1:50,X_ekf(1,:)-data_r(2,:),'k');
xlabel('时间(s)');ylabel('位置误差(m)');
title('X方向位置误差');
subplot(1,2,2);grid on;
plot(0:0.1:50,X_ekf(4,:)-data_r(3,:),'k');
xlabel('时间(s)');ylabel('位置误差(m)');
title('Y方向位置误差');
figure
subplot(1,2,1);grid on;
plot(0:0.1:50,X_ekf(2,:)-data_r(4,:),'k');
xlabel('时间(s)');ylabel('速度误差(m/s)');
title('X方向速度误差');
subplot(1,2,2);grid on;
plot(0:0.1:50,X_ekf(5,:)-data_r(5,:),'k');
xlabel('时间(s)');ylabel('速度误差(m/s)');
title('Y方向速度误差');
figure
subplot(1,2,1);grid on;
plot(0:0.1:50,X_ekf(3,:)-data_r(6,:),'k');
xlabel('时间(s)');ylabel('加速度误差(m/s^2)');
title('X方向加速度误差');
subplot(1,2,2);grid on;
plot(0:0.1:50,X_ekf(6,:)-data_r(7,:),'k');
xlabel('时间(s)');ylabel('加速度误差(m/s^2)');
title('Y方向加速度误差');
figure
subplot(1,2,1);grid on;
plot(0:0.1:50,data_r(8,:),'r');hold on;plot(0:0.1:50,sqrt(X_ekf(1,:).^2+X_ekf(4,:).^2),'k');
xlabel('时间(s)');ylabel('距离(m)');legend('真值','EKF滤波值');
title('目标距离真值与滤波值');
subplot(1,2,2);grid on;
plot(0:0.1:50,data_r(9,:),'r');hold on;plot(0:0.1:50,atan(X_ekf(4,:) ./ ((-1).^(X_ekf(1,:) < 0) .* X_ekf(1,:))),'k');
xlabel('时间(s)');ylabel('角度(rad)');legend('真值','EKF滤波值');
title('目标角度真值与滤波值');
figure
subplot(1,2,1);grid on;
plot(0:0.1:50,data_r(8,:) - sqrt(X_ekf(1,:).^2+X_ekf(4,:).^2),'k');
xlabel('时间(s)');ylabel('距离误差(m)');
title('目标距离真值与滤波值误差');
subplot(1,2,2);grid on;
plot(0:0.1:50,data_r(9,:) - atan(X_ekf(4,:) ./ ((-1).^(X_ekf(1,:) < 0) .* X_ekf(1,:))),'k');
xlabel('时间(s)');ylabel('角度误差(rad)');
title('目标角度真值与滤波值误差');
save('ekf_ca','X_ekf');
评论1