%%?本程序仿真2維運(yùn)動(dòng)的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]）;
%?狀態(tài)轉(zhuǎn)移矩陣
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];

%%?測量結(jié)果
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;????%?跟蹤的初始狀態(tài)

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

for?k=2:N
????X_pre（:k）=A*X_est（:k-1）;??%?狀態(tài)轉(zhuǎn)移過程應(yīng)該加入噪聲???
????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

?屬性????????????大小?????日期????時(shí)間???名稱
-----------?---------??----------?-----??----

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

-----------?---------??----------?-----??----

?????????????????2341????????????????????1