close?all;
clear?all;
%%??真實軌跡模擬
kx?=?.01;???ky?=?.05;???????%?阻尼系數
g?=?9.8;????????????????????%?重力
t?=?15;?????????????????????%?仿真時間
Ts?=?0.1;???????????????????%?采樣周期?
len?=?fix（t/Ts）;????????????%?仿真步數
dax?=?3;?day?=?3;???????%?系統噪聲
X?=?zeros（len4）;?
X（1:）?=?[0?50?500?0];?%?狀態模擬的初值
for?k=2:len
????x?=?X（k-11）;?vx?=?X（k-12）;?y?=?X（k-13）;?vy?=?X（k-14）;?
????x?=?x?+?vx*Ts;
????vx?=?vx?+?（-kx*vx^2+dax*randn（11））*Ts;
????y?=?y?+?vy*Ts;
????vy?=?vy?+?（ky*vy^2-g+day*randn（1））*Ts;
????X（k:）?=?[x?vx?y?vy];
end
%%??構造量測量
dr?=?8;??dafa?=?0.1;????????%?量測噪聲
for?k=1:len
????r?=?sqrt（X（k1）^2+X（k3）^2）?+?dr*randn（11）;
????a?=?atan（X（k1）/X（k3））*57.3?+?dafa*randn（11）;
????Z（k:）?=?[r?a];
end
%%?ekf?濾波
Qk?=?diag（[0;?dax/10;?0;?day/10]）^2;
Rk?=?diag（[dr;?dafa]）^2;
Pk?=?10*eye（4）;
Pkk_1?=?10*eye（4）;
x_hat?=?[0404000]‘;
X_est?=?zeros（len4）;
x_forecast?=?zeros（41）;
z?=?zeros（41）;
for?k=1:len
????%?1?狀態預測????
????x1?=?x_hat（1）?+?x_hat（2）*Ts;
????vx1?=?x_hat（2）?+?（-kx*x_hat（2）^2）*Ts;
????y1?=?x_hat（3）?+?x_hat（4）*Ts;
????vy1?=?x_hat（4）?+?（ky*x_hat（4）^2-g）*Ts;
????x_forecast?=?[x1;?vx1;?y1;?vy1];????????%預測值
????%?2??觀測預測
????r?=?sqrt（x1*x1+y1*y1）;
????alpha?=?atan（x1/y1）*57.3;
????y_yuce?=?[ralpha]‘;
????%??狀態矩陣
????vx?=?x_forecast（2）;??vy?=?x_forecast（4）;
????F?=?zeros（44）;
????F（11）?=?1;??F（12）?=?Ts;
????F（22）?=?1-2*kx*vx*Ts;
????F（33）?=?1;??F（34）?=?Ts;
????F（44）?=?1+2*ky*vy*Ts;
????Pkk_1?=?F*Pk*F‘+Qk;
????%?觀測矩陣
????x?=?x_forecast（1）;?y?=?x_forecast（3）;
????H?=?zeros（24）;
????r?=?sqrt（x^2+y^2）;??xy2?=?1+（x/y）^2;
????H（11）?=?x/r;??H（13）?=?y/r;
????H（21）?=?（1/y）/xy2;??H（23）?=?（-x/y^2）/xy2;
????
????Kk?=?Pkk_1*H‘*（H*Pkk_1*H‘+Rk）^-1;???????%計算增益
????x_hat?=?x_forecast+Kk*（Z（k:）‘-y_yuce）;??????%校正
????Pk?=?（eye（4）-Kk*H）*Pkk_1;
????X_est（k:）?=?x_hat;
end
%%?
figure?hold?on?grid?on;
plot（X（:1）X（:3）‘-b‘）;
plot（Z（:1）.*sin（Z（:2）*pi/180）?Z（:1）.*cos（Z（:2）*pi/180））;
plot（X_est（:1）X_est（:3）?‘r‘）;
xlabel（‘X‘）;?
ylabel（‘Y‘）;?
title（‘EKF?simulation‘）;
legend（‘real‘?‘measurement‘?‘ekf?estimated‘）;
axis（[-5230290530]）;

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

?????文件???????2227??2014-11-04?22:47??ekf.m

?????文件???????1476??2010-02-13?20:30??fusion.m

?????文件????????756??2010-02-13?20:29??kalman.m

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

?????????????????4459????????????????????3