clear;??
clc;

%%??真實軌跡模擬??
kx?=?.01;???ky?=?.05;???????%?阻尼系數??
g?=?9.8;????????????????????%?重力??
t?=?15;?????????????????????%?仿真時間??
Ts?=?0.1;???????????????????%?采樣周期???
len?=?fix（t/Ts）;????????????%?仿真步數??
dax?=?3;?day?=?3;???????????%?系統噪聲??X、Y兩個狀態下的噪聲
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];????????%預測值??
????
????%?觀測量預測??
????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;????????????????????%公式2，誤差協方差預測
????
????%?觀測矩陣??
????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;???????%公式3，計算增益K??
????x_hat?=?x_forecast+Kk*（Z（k:）‘-y_yuce）;?%公式4，狀態更新??
????Pk?=?（eye（4）-Kk*H）*Pkk_1;???????????????%公式5，誤差協方差更新?
????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]）;

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????文件????????2010??2018-06-22?20:00??example2_KF.m
?????文件????????1522??2018-06-22?21:06??example3_KF.m
?????文件????????2870??2018-06-22?11:14??example1_EKF.m