clc;close?all;clear;
T?=?2;?%采樣周期
%?sigmaQ?=?100;
sigmaR?=?200;
N?=?100;
F=[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];%狀態轉移矩陣
H?=?[1?0?0?0?0?0;
????0?0?0?1?0?0];%量測矩陣
R?=?[sigmaR^2?0;
????0?sigmaR^2];%量測協方差

qq11?=?T^5/20;
qq12?=?T^4/8;
qq13?=?T^3/6;
qq22?=?T^3/3;
qq23?=?T^2/2;
qq33?=?T;
Q=[qq11?qq12?qq13?0????0????0;
???qq12?qq22?qq23?0????0????0;
???qq13?qq23?qq33?0????0????0;
???0????0????0????qq11?qq12?qq13;
???0????0????0????qq12?qq22?qq23;
???0????0????0????qq13?qq23?qq33];%過程噪聲協方差

%初始化跟蹤曲線
X?=?track2D（0.1N）;
X?=?[X（1:2:）;0.1*randn（1N）;X（3:4:）;0.14*randn（1N）];
Z?=?[X（1:）;X（4:）];
%?Z?=?X;
%?X?=?reshape（X61100）;

%?%直角坐標轉極坐標
%?for?i?=?1:N
%?????d?=?sqrt（X（1i）^2+X（4i）^2）;
%?????sita?=?atan（X（4i）/X（1i））;
%?end
%?%極坐標測量誤差
%?d?=?d?+?sigmaR*randn（1N）;
%?sita?=?sita?+?0.001*randn（1N）;
%?%極坐標轉直角坐標
%?for?i?=?1:N
%?????Z（1i）?=?d（1i）*cos（sita（1i））;
%?????Z（2i）?=?d（1i）*sin（sita（1i））;
%?end
X?=?reshape（X61N）;
Z?=?Z?+?sigmaR*randn（2N）;
Z?=?reshape（Z21N）;%測量數據
for?i?=?1:N
????xtr（i）?=?Z（11i）;
????ytr（i）?=?Z（21i）;
end
plot（xtrytr‘.-‘）;
%?xe（::1）?=?Z（::1）;
xe（::2）?=?[Z（112）（Z（112）-Z（111））/T2*（Z（112）-Z（111））/T^2...
????Z（212）（Z（212）-Z（211））/T2*（Z（212）-Z（211））/T^2]‘;
xp（::1）?=?zeros（61）;
xp（::2）?=?zeros（61）;

%濾波協方差初始化
P11=R（11）;
P12=R（11）/T;
P13=R（11）/T^2;
P22=2*R（11）/T^2;
P23=3*R（11）/T^3;
P33=6*R（11）/T^4;
P44=R（22）;
P45=R（22）/T;
P46=R（22）/T^2;
P55=2*R（22）/T^2;
P56=3*R（22）/T^3;
P66=6*R（22）/T^4;
p（::2）=[P11?P12?P13?0???0???0;
????P12?P22?P23?0???0???0;
????P13?P23?P33?0???0???0;
????0???0???0???P44?P45?P46;
????0???0???0???P45?P55?P56;
????0???0???0???P46?P56?P66];

for?k?=?3:N
????xp（::k）?=?F*xe（::k-1）;
????a（::k）?=?Z（::k）-H*xp（::k）;%新息
????pt（::k）?=?F*p（::k-1）*F‘+Q;
????A（::k）?=?H*pt（::k）*H‘+R;
????K（::k）?=?pt（::k）*H‘*inv（A（::k））;
????
????xe（::k）?=?xp（::k）+K（::k）*a（::k）;
????p（::k）?=?（eye（6）-K（::k）*H）*pt（::k）;
end
for?i?=?1:N;
????xtr（i）?=?xe（11i）;
????ytr（i）?=?xe（41i）;
end
plot（xtrytr‘.-‘）;
legend（‘真軌跡‘‘測量軌跡‘‘跟蹤軌跡‘）;?

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----

?????文件???????2318??2018-09-08?10:42??main.m

?????文件????????531??2018-09-08?10:41??track2D.m

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

?????????????????2849????????????????????2