%使用卡爾曼濾波方法對飛行航班進行軌跡預測
%數(shù)據(jù)來源：FlightAware（https://zh.flightaware.com）
%航????班：CES9937?????寧波櫟社國際機場飛往成都雙流國際機場
%飛行時間：2018-07-17??07:07-09:55
%說明：取起飛后前20組數(shù)據(jù)作為實驗數(shù)據(jù)。對時間點進行近似取值，假設每隔30s上報一次數(shù)據(jù)
clear;
clc;
%采樣點的個數(shù)
N=228;
%測試數(shù)據(jù)：緯度
latitude=load（‘C:\workspace\matlab?workspace\CES9937\latitude.txt‘）;
%系統(tǒng)噪聲方差
Q=0.25;
%觀測噪聲方差
R=0.01;
%產(chǎn)生系統(tǒng)噪聲
W=sqrt（Q）*randn（1N）;
%產(chǎn)生測量噪聲
V=sqrt（R）*randn（1N）;
%定義符號變量t
syms?t;
%方程-根據(jù)二階擬合，緯度關于時間t的函數(shù)
%lat?=?0.000129*t^2?-?0.022433*t?+?29.819;
%方程-根據(jù)三階擬合，緯度關于時間t的函數(shù)
%lat?=?（-1.4341e-06）*t^3?+?（0.00062162）*t^2?+?（-0.067656）*t?+??30.691;
%方程-根據(jù)六階擬合，緯度關于時間t的函數(shù)
lat?=?（2.5716e-12）*t^6?+?（-1.5634e-09）*t^5?+?（3.2239e-07）*t^4?+?（-2.427e-05）*t^3?+?（0.00037919）*t^2?+?（-0.0017116）*t?+?29.728;
%一階泰勒展開
%lat_taylor?=?taylor（latt1）
%求解雅克比矩陣作為狀態(tài)轉移矩陣
lat_jacobian?=?jacobian（latt）;
%觀測矩陣
H?=?1;
%根據(jù)狀態(tài)方程，計算緯度lat_x
lat_x?=?zeros（1N）;
%設置初始值
lat_x（1）?=?29.8000;
%在實驗中，假設觀測值就是真實值
for?k=2:N
????lat_x（k）?=?latitude（k）;
end
%EKF濾波算法
%卡爾曼對緯度的濾波結果
lat_ekf?=?zeros（1N）;
%假設濾波初始值和假設的初始值相同
lat_ekf（1）?=?lat_x（1）;
%協(xié)方差矩陣初始值
P0?=?eye（1）;
%觀測值
lat_obse?=?zeros（1N）;
%設置觀測初值
lat_obse（1）?=?lat_x（1）?+?V（1）;
for?k?=?2:N
????%系統(tǒng)狀態(tài)方程求取理論值
????lat_theo?=?subs（lattk）;
????lat_obse（k）?=?lat_theo?+?V（k）;
????%狀態(tài)轉移矩陣F
????F?=?subs（lat_jacobiantk）;
????%協(xié)方差預測
????P?=?F*P0*F‘?+?Q;
????%求卡爾曼增益
????K?=?P*H‘*inv（H*P*H‘?+?R）;
????%狀態(tài)更新
????lat_ekf（k）?=?lat_theo?+?K*（lat_x（k）?-?lat_obse（k））;
????%協(xié)方差陣更新
????P0?=?（eye（1）?-?K*H）*P;
end
%誤差分析
%濾波與真實值之間的偏差
Err_Kalman?=?zeros（1N）;
%測量值與真實值之間的偏差
Err_Messure=zeros（1N）;

for?k=1:N
????Err_Kalman（k）?=?abs（lat_ekf（k）?-?lat_x（k））;
????Err_Messure（k）?=?abs（lat_obse（k）?-?lat_x（k））;
end
%濾波效果圖
t?=?1:N;
figure
hold?on;
box?on;
plot（tlat_x‘g-‘tlat_obse‘b-‘tlat_ekf‘r-‘）;
legend（‘真實值‘‘觀測值‘‘kalman濾波值‘）;
xlabel（‘測報時間點‘）;
ylabel（‘緯度值‘）;
%誤差分析圖
figure
hold?on;
box?on;
plot（tErr_Kalman‘r-‘）;
legend（‘kalman濾波偏差‘）;
xlabel（‘測報時間點‘）;
ylabel（‘緯度偏差值‘）;
%誤差分析圖
figure
hold?on;
box?on;
plot（tErr_Messure‘b-‘）;
legend（‘系統(tǒng)狀態(tài)方程偏差‘）;
xlabel（‘測報時間點‘）;
ylabel（‘緯度偏差值‘）;

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

?????文件???????2523??2018-08-02?10:42??QA_EKF_lat.m

?????文件?????153993??2018-07-24?16:28??CES9937\CES9937.docx

?????文件???????1825??2018-07-17?15:07??CES9937\latitude.txt

?????文件???????2051??2018-07-17?15:15??CES9937\longitude.txt

?????文件????????410??2018-07-17?11:16??CES9937\飛行數(shù)據(jù)-20組.txt

?????目錄??????????0??2018-08-14?20:18??CES9937

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

???????????????160802????????????????????6