資源簡介
簡單模擬了kalman跟蹤勻速直線運動的情況,適合初學者使用。
代碼片段和文件信息
%?????設(shè)置初始化信息
%??N:設(shè)置卡爾曼濾波器追蹤點數(shù)
%??r:設(shè)置估計變量個數(shù),這里r=3
%??s:被追蹤的火箭的距離,初始值為1000m
%??v:火箭的速度,初始值為50m/s
%??a:火箭的加速度,初始值為20m/s2,此時加速度默認為不變
%??T:?雷達的掃描間隔,此時設(shè)為1秒
%??wt:?系統(tǒng)噪聲,方差為20
%??vt:?量測噪聲,方差為16
%航跡噪聲與測量噪聲不相關(guān)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear?all;
close?all;
N?=?100;
T?=?1;
t?=?1:N/T;
num=50?;%mentocarlo仿真次數(shù)
qk=2;
Qk=qk*eye(2);
var?=?10;
Rk?=?var^2;????%量測噪聲,方差為16
%%
%生成真實目標運動軌跡
[sx_realsy_realsz_realRMSE_xk]=trueTrack(NT);
A?=?[1?T?;???0?1];
H?=?[1?0?];
%?A?=?[1?T?0?0;
%?????????0?1?0?0;
%?????????0?0?1?T;
%?????????0?0?0?1];
%?H?=?[1?0?0?0;?0?0?1?0];
P0?=?var^2?*eye(2);
P1_x?=?P0;
P1_y?=?P0;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%?卡爾曼濾波部分,繼承之前初始化變量
%?A:轉(zhuǎn)移矩陣
%?H:量測矩陣
%?Qk:系統(tǒng)噪聲矩陣
%?Rk:量測噪聲矩陣
%?P0:均方誤差矩陣初始值
%?Y:火箭的狀態(tài)矩陣,由k_s,k_vk_a組成
%?Y0:狀態(tài)矩陣的初始值
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for?m=1:num
????sx_measure?=?sx_real?+?var?*?rand(1N/T);?
????sy_measure?=?sy_real?+?var?*?rand(1N/T);?
???%?sz_measure?=?sz_real?+?var?*?rand(1N/T);?????%引入測量噪聲
????xks(1)=sx_measure(1);?
????yks(1)=sy_measure(1);
????xks(2)=sx_measure(2);
????yks(2)=sy_measure(2);
????vx?=?(xks(2)?-?xks(1))/T;
????vy?=?(yks(2)?-?yks(1))/T;
??
????Zkx=[xks(1);vx];
????Zky=[yks(
評論
共有 條評論