二维Kalman滤波

本程序仿真了2D直角坐标下目标运动的kalman跟踪，并对其性能进行分析。

%% 本程序仿真2维运动的Kalman滤波

close all
clear
clc

%% 
dt=1;
N=100;
t=（0:N-1）*dt;

x0=80;
y0=0;
Vx=5;
Vy=4;

x_real=x0+Vx*t;
y_real=y0+Vy*t;
vx_real=Vx*ones（1N）;
vy_real=Vy*ones（1N）;

X_real=[x_real;vx_real;y_real;vy_real];

sigma_x=0.1;
sigma_vx=0.0;
sigma_y=0.1;
sigma_vy=0.0;

Q=diag（[sigma_xsigma_vxsigma_ysigma_vy]）;
% 状态转移矩阵
A=[1 dt 0 0;        
   0  1  0 0;
   0  0  1 dt;
   0  0  0  1];

H=[1 0 0 0;
   0 0 1 0];

%% 测量结果
sigma_x_m=3;
sigma_y_m=3;
R=diag（[sigma_x_msigma_y_m]）;

x_mea=x_real+sigma_x_m*randn（1N）;
y_mea=y_real+sigma_y_m*randn（1N）;

X_mea=[x_mea;y_mea];

X_est=zeros（4N）;
X_pre=zeros（4N）;

% X_est0=[x0Vxy0Vy]‘;
X_est0=[x00y00]‘;

X_est（:1）=X_est0;    % 跟踪的初始状态

% P=zeros（4）;
P=diag（[2 1 2 3]）;
I=eye（4）;

for k=2:N
    X_pre（:k）=A*X_est（:k-1）;  % 状态转移过程应该加入噪声???
    P_pre=A*P*A‘+Q;
    
    % 测量更新过程
    K=P_pre*H‘*inv（H*P_pre*H‘+R）;
    X_est（:k） = X_pre（:k）+K*（X_mea（:k）-H*X_pre（:k））;
    P=（I-K*H）*P_pre;
end


figure
hold on
plot（X_real（1:）‘b‘）
plot（X_mea（1:）‘k‘）
plot（X_est（1:）‘r‘）
legend（‘Real‘‘Measure‘‘Estimate‘）
title（‘X coordinate‘）

figure
hold on
plot（X_real（3:）‘b‘）
plot（X_mea（2:）‘k‘）
plot（X_est（3:）‘r‘）
legend（‘Real‘‘Measure‘‘Estimate‘）
title（‘Y coordinate‘）


figure
hold on
plot（X_real（1:）X_real（3:）‘b‘）
plot（X_mea（1:）X_mea（2:）‘k‘）
plot（X_est（1:）X_est（3:）‘r‘）
legend（‘Real‘‘Measure‘‘Estimate‘）
title（‘Kalman tracking in 2D plane‘）
xlabel（‘x‘）
ylabel（‘y‘）


% 
% 
% for i=2:L
%     x_mea=Z（:i）;
%     [x_fP_f]=fun_kalman_1D（x_preP_prex_meaAQHRE）;
%     X（:i）=x_f;
%     P（::i）=P_f;
%     
%     x_pre=x_f;
%     P_pre=P_f;
% end
% 
% 
% figure
% plot（x_real‘k‘）
% hold on
% plot（Z（1:）‘bo‘）
% hold on
% plot（X（1:）‘r+‘）
% legend（‘Expected‘‘Measure‘‘Filter‘）
% 
% 
% figure
% plot（vx*ones（1L）‘k‘）
% hold on
% plot（vx_mea‘bo‘）
% hold on
% plot（X（2:）‘r+‘）
% legend（‘expectation value‘‘measurement value‘‘filter output‘‘location‘‘northwest‘）
% 
% 
% % function [x_fP]=fun_kalman_1D（x_preP_prex_meaAQHRE）
% % 
% % x_t=A*x_pre;
% % P_t=A*P_pre*A‘+Q;
% % G=P_t*H‘*inv（R+H*P_t*H‘）;
% % x_f=x_t+G*（x_mea-H*x_t）;
% % P=（E-G*H）*P_t;
% % end

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       2341  2018-08-13 15:28  kalman_2D_test.m

----------- ---------  ---------- -----  ----

                 2341                    1