%??Kalman濾波在船舶GPS導航定位系統中的應用
function?main
clc;clear;
T=1;%雷達掃描周期
N=80/T;%總的采樣次數
X=zeros（4N）;%目標真實位置、速度
X（:1）=[-100220020];%目標初始位置、速度
Z=zeros（2N）;%傳感器對位置的觀測
Z（:1）=[X（11）X（31）];%觀測初始化
delta_w=1e-2;%如果增大這個參數，目標真實軌跡就是曲線
Q=delta_w*diag（[0.510.51]）?;%過程噪聲均值
R=100*eye（2）;%觀測噪聲均值
F=[1T00;0100;001T;0001];%狀態轉移矩陣
H=[1000;0010];%觀測矩陣
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for?t=2:N
????X（:t）=F*X（:t-1）+sqrtm（Q）*randn（41）;%目標真實軌跡
????Z（:t）=H*X（:t）+sqrtm（R）*randn（21）;?%對目標觀測
end
%卡爾曼濾波
Xkf=zeros（4N）;
Xkf（:1）=X（:1）;%卡爾曼濾波狀態初始化
P0=eye（4）;%協方差陣初始化
for?i=2:N
????Xn=F*Xkf（:i-1）;%預測
????P1=F*P0*F‘+Q;%預測誤差協方差
????K=P1*H‘*inv（H*P1*H‘+R）;%增益
????Xkf（:i）=Xn+K*（Z（:i）-H*Xn）;%狀態更新
????P0=（eye（4）-K*H）*P1;%濾波誤差協方差更新
end
%誤差更新
for?i=1:N
????Err_Observation（i）=RMS（X（:i）Z（:i））;%濾波前的誤差
????Err_KalmanFilter（i）=RMS（X（:i）Xkf（:i））;%濾波后的誤差
end
%畫圖
figure
hold?on;box?on;
plot（X（1:）X（3:）‘-k‘）;%真實軌跡
plot（Z（1:）Z（2:）‘-b.‘）;%觀測軌跡
plot（Xkf（1:）Xkf（3:）‘-r+‘）;%卡爾曼濾波軌跡
legend（‘真實軌跡‘‘觀測軌跡‘‘濾波軌跡‘）
figure
hold?on;?box?on;
plot（Err_Observation‘-ko‘‘MarkerFace‘‘g‘）
plot（Err_KalmanFilter‘-ks‘‘MarkerFace‘‘r‘）
legend（‘濾波前誤差‘‘濾波后誤差‘）
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%計算歐式距離子函數
function?dist=RMS（X1X2）;
if?length（X2）<=2
????dist=sqrt（?（X1（1）-X2（1））^2?+?（X1（3）-X2（2））^2?）;
else
????dist=sqrt（?（X1（1）-X2（1））^2?+?（X1（3）-X2（3））^2?）;
end
%%%%%%%%%%%%%%%%%%%%%%%%

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????目錄???????????0??2018-12-21?16:31??KF?Boat_ZKX\
?????文件????????1656??2018-12-19?20:12??KF?Boat_ZKX\BoatCSDN.m
?????文件???????26240??2018-12-21?16:31??KF?Boat_ZKX\Error?Analysis.fig
?????文件???????34517??2018-12-21?16:31??KF?Boat_ZKX\KF?Boat?Tracking.fig
?????文件????????2485??2018-12-21?16:31??KF?Boat_ZKX\KF_BoatLocation_ZKX.asv
?????文件????????2492??2018-12-21?16:31??KF?Boat_ZKX\KF_BoatLocation_ZKX.m
?????文件??????????73??2018-12-19?20:17??KF?Boat_ZKX\RMSE.m