?function?main?
clc;
clear;??
T=1;%掃描周期
N=120/T;%采樣次數(shù)??
X=zeros（4N）;%目標真實位置、速度?
X（:1）=[302402];%目標初始位置（3040）速度（220）
Z=zeros（2N）;%基站對位置的觀測?
a=exprnd（214）;%指數(shù)分布的過程噪聲
Q=diag（a）;%過程噪聲
b=exprnd（312）;
R=60*diag（b）;?%觀測噪聲均值?指數(shù)分布
A=[1T00;
???0100;
???001T;
???0001];?%狀態(tài)轉(zhuǎn)移矩陣?
G=[1000;
???0010];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%?
for?t=2:N??????
????X（:t）=A*X（:t-1）+sqrtm（Q）*randn（41）;%目標真實軌跡?????
end
MX=X（1:）;
MY=X（3:）;
for?t=1:N
????yuce=zz（MX（1t）MY（1t））;
????Z（:t）=[yuce（1）;yuce（2）]+sqrtm（R）*randn（21）;%對目標的觀測
end
%?Kalman濾波?
Xkf=zeros（4N）;?%濾波后的最優(yōu)估計?
Xkf（:1）=X（:1）;%初始化?
P0=eye（4）;%?誤差協(xié)方差陣初始化?
for?i=2:N??????
????%預測方程
????Xn=A*Xkf（:i-1）;%狀態(tài)預測?????
????P1=A*P0*A‘+Q;%預測誤差協(xié)方差??
????%信息方程
????K=P1*G‘/（G*P1*G‘+R）;%Kalman增益
????%估計方程
????Xkf（:i）=Xn+K*（Z（:i）-G*Xn）;%狀態(tài)更新?????
????P0=（eye（4）-K*G）*P1;%濾波誤差協(xié)方差更新?
end
%?誤差分析?
for?i=1:N??????
????Observation（i）=RMS（X（:i）Z（:i））;%濾波前的誤差?????
????KalmanFilter（i）=RMS（X（:i）Xkf（:i））;%濾波后的誤差?
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%?
%?畫圖?
figure?
hold?on;box?on;??
plot（X（1:）X（3:）‘-k‘）;%真實軌跡?
plot（Z（1:）Z（2:）‘*‘）;%觀測軌跡??
plot（Xkf（1:）Xkf（3:）‘-r‘）;%Kalman濾波軌跡?
legend（‘真實軌跡‘‘觀測軌跡‘‘濾波后軌跡‘）?
xlabel（‘橫坐標?X/m‘）;?ylabel（‘縱坐標?Y/m‘）;?
figure?
hold?on;box?on;??
plot（Observation‘-ko‘‘MarkerFace‘‘g‘）?
plot（KalmanFilter‘-ks‘‘MarkerFace‘‘r‘）?
legend（‘濾波前誤差‘‘濾波后誤差‘）
xlabel（‘觀測時間/s‘）;?ylabel（‘誤差值‘）;?

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

?????文件???????1690??2017-11-19?10:34??卡爾曼定位程序\chan\main.m

?????文件????????178??2017-11-06?11:06??卡爾曼定位程序\chan\RMS.m

?????文件???????2522??2017-11-19?11:04??卡爾曼定位程序\chan\zz.m

?????文件???????1109??2017-11-19?11:30??卡爾曼定位程序\fang\Fang.m

?????文件???????1723??2017-11-17?19:23??卡爾曼定位程序\fang\main.m

?????文件????????178??2017-11-06?11:06??卡爾曼定位程序\fang\RMS.m

?????文件???????1727??2017-11-17?19:42??卡爾曼定位程序\SI\main.m

?????文件????????178??2017-11-06?11:06??卡爾曼定位程序\SI\RMS.m

?????文件???????1107??2017-11-19?11:50??卡爾曼定位程序\SI\SI.m

?????文件???????1730??2017-11-17?19:57??卡爾曼定位程序\Taylor\main.m

?????文件????????178??2017-11-06?11:06??卡爾曼定位程序\Taylor\RMS.m

?????文件???????1342??2017-11-19?12:07??卡爾曼定位程序\Taylor\Taylor.m

?????目錄??????????0??2018-01-02?21:15??卡爾曼定位程序\chan

?????目錄??????????0??2018-01-02?21:15??卡爾曼定位程序\fang

?????目錄??????????0??2018-01-02?21:15??卡爾曼定位程序\SI

?????目錄??????????0??2018-01-02?21:15??卡爾曼定位程序\Taylor

?????目錄??????????0??2018-01-02?21:15??卡爾曼定位程序

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

????????????????13662????????????????????17