%xy軸方向——二維估計
clc
close?all
clear?all

%參數
T=1;??????????????????????????????????%采樣間隔
q1=1;?????????????????????????????????%x過程噪聲方差E[v^2（k）=q1]
q2=1;?????????????????????????????????%y過程噪聲方差E[v^2（k）=q2]
r=1;??????????????????????????????????%量測噪聲方差E[W^2（k）=r=1]
F=[1?T?0?0;0?1?0?0;0?0?1?T;0?0?0?1];??%狀態轉移矩陣
C=[T^2/2?0;T?0;0?T^2/2;0?T];??????????%過程噪聲分布矩陣
H=[1?0?0?0;0?0?1?0];??????????????????%量測矩陣

%初始化
X（:1）=[10;2;10;2];???????????%目標真實初始狀態
W=normrnd（0r21）;?????????????%量測噪聲
Z（:1）=H*X（:1）+W;??????????????%量測方程Z（k）=H（k）*X（k）+W（k）
v1=normrnd（0q111）;???????????%x過程噪聲
v2=normrnd（0q211）;???????????%y過程噪聲
X（:2）=F*X（:1）+C*[v1;v2];??????%狀態方程X（k+1）=F（k）*X（k）+C（k）*v（k）
W=normrnd（0r21）;
Z（:2）=H*X（:2）+W;

%四維狀態向量估計的初始化
XX（:1）=[Z（12）;（Z（12）-Z（11））/T;Z（22）;（Z（22）-Z（21））/T];????????%初始狀態估計
theta=atan（（Z（22）-Z（21））/（Z（12）-Z（11）））;
L=sqrt（（Z（22）-Z（21））^2+（Z（12）-Z（11））^2）;
A=[cos（theta）?-L*sin（theta）;sin（theta）?L*cos（theta）];
R=A*[2?0;0?pi/180]*A‘;??????????????????????????????????????????????%徑向距離方差為2，方位角測量誤差方差為1°
P（:1:4）=[R（11）??????R（11）/T???????R（12）???????R（12）/T
??????????R（11）/T????2*R（11）/（T^2）?R（12）/T?????2*R（12）/（T^2）
??????????R（12）??????R（12）/T???????R（22）???????R（22）/T
??????????R（12）/T????2*R（12）/（T^2）?R（22）/T?????2*R（22）/（T^2）];??%初始協方差

%卡爾曼濾波??????
for?k=1:999
????
????XX1（:k+1）=F*XX（:k）;???????????????%狀態的一步預測XX1（k+1|k）=F*XX（k|k）+G*u
????ZZ（:k+1）=H*XX1（:k+1）;?????????????%量測的預測ZZ（k+1|k）=H*XX1（k+1|k）
????
????v1=normrnd（0q111）;???????????????%x過程噪聲
????v2=normrnd（0q211）;???????????????%y過程噪聲
????X（:k+2）=F*X（:k+1）+C*[v1;v2];??????%狀態方程
????W=normrnd（0r21）;????
????Z（:k+2）=H*X（:k+2）+W;??????????????%量測方程
????U（:k+1）=Z（:k+2）-ZZ（:k+1）;????????%新息U（k）=Z（k+1）-ZZ（k+1|k）
????
????PP（:4*k+1:4*k+4）=F*P（:4*k-3:4*k）*F‘+C*[q1?0;0?q2]*C‘;???%協方差的一步預測PP（k+1|k）=F*P（k|k）*F‘+Q（k）協方差定值
????theta=atan（（Z（2k+2）-Z（2k+1））/（Z（1k+2）-Z（1k+1）））;
????L=sqrt（（Z（2k+2）-Z（2k+1））^2+（Z（1k+2）-Z（1k+1））^2）;
????A=[cos（theta）?-L*sin（theta）;sin（theta）?L*cos（theta）];
????R=A*[2?0;0?3]*A‘;?????????????????????????????????????????%k時刻量測噪聲協方差
????S（:2*k:2*k+1）=H*PP（:4*k+1:4*k+4）*H‘+R;%[r?0;0?r];???????%新息協方差S（k+1）=H*PP（k+1|k）*H‘+R（k+1），協方差定值
?
????
????K（:2*k:2*k+1）=PP（:4*k+1:4*k+4）*H‘/S（:2*k:2*k+1）;???????%增益K（k+1）=PP（k+1|k）*H‘/S（k+1）
????
????XX（:k+1）=XX1（:k+1）+K（:2*k:2*k+1）*U（:k+1）;?????????????%狀態更新方程XX（k+1|k+1）=XX1（k+1|k）+K（k+1）*U（k+1）
????P（:4*k+1:4*k+4）=PP（:4*k+1:4*k+4）-K（:2*k:2*k+1）*S（:2*k:2*k+1）*K（:2*k:2*k+1）‘;?%協方差更新方程P（k+1|k+1）=PP（k+1|k）-K*S*K‘
?????
end

for?i=1:999
????D（1i）=PP（14*i-3）;????%X預測位置
????D（2i）=P（14*i-3）;?????%X更新位置
????D（3i）=PP（34*i-1）;????%Y預測位置
????D（4i）=P（34*i-1）;?????%Y更新位置
????V（1i）=PP（24*i-2）;????%X預測速度
????V（2i）=P（24*i-2）;?????%X更新速度
????V（3i）=PP（44*i）;??????%Y預測速度
????V（4i）=P（44*i）;???????%Y更新速度
end

for?i=2:999
????DD（1i）=sqrt（（?D（1i）^2）*（?D（3i）^2））;?????????????????

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

?????文件???????4964??2017-03-01?08:25??kalman.m

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

?????????????????4964????????????????????1