%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%???交互多模Kalman濾波在目標跟蹤中的應用、GPS測試的折線路段
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function?ImmKalman


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear
clc
[GGATimeLATNLONESTATUSQUANTITYHDOPAltSMAltEma]?=?textread（‘宿舍——裕民大門22.txt‘‘%s?%f?%f?%s?%f?%s?%f?%f?%f?%f?%s?%f?%s?%s‘‘delimiter‘?‘‘）;?%#ok

LAT=LAT（LAT~=0）;?
LON=LON（LON~=0）;

lat?=?floor（LAT/100）?+?（LAT?-?floor（LAT/100）*100）/60;???%換算緯度
lon?=?floor（LON/100）?+?（LON?-?floor（LON/100）*100）/60;???%換算經度

?%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
?
T=2;
M=5;
N=900/T;
N1=400/T;
N2=600/T;
N3=610/T;
N4=660/T;
Delta=100;
Rx=zeros（N1）;
Ry=zeros（N1）;
Zx=zeros（NM）;??
Zy=zeros（NM）;



t=2:T:400;
%?x0=2000+0*t‘;
x0=lat（t/2:）;
%y0=10000-15*t‘;
y0=lon（t/2:）;


t=402:T:600;
%?x1=x0（N1）+0.075*（（t‘-400）.^2）/2;
x1=lat（t/2:）;
%?y1=y0（N1）-15*（t‘-400）+0.075*（（t‘-400）.^2）/2;
y1=lon（t/2:）;

t=602:T:610;
%vx=0.075*（600-400）;
%?x2=x1（N2-N1）+vx*（t‘-600）;
x2?=?lat（t/2:）;
%?y2=y1（N2-N1）+0*t‘;
y2?=?lon（t/2:）;

t=612:T:660;
%x3=x2（N3-N2）+（vx*（t‘-610）-0.3*（（t‘-610）.^2）/2）;
x3?=?lat（t/2:）;
%?y3=y2（N3-N2）-0.3*（（t‘-610）.^2）/2;
y3?=?lon（t/2:）;

t=662:T:900;
%?vy=-0.3*（660-610）;
%?x4=x3（N4-N3）+0*t‘;
x4?=?lat（t/2:）;
%?y4=y3（N4-N3）+vy*（t‘-660）;
y4?=?lon（t/2:）;


Rx=[x0;x1;x2;x3;x4];
Ry=[y0;y1;y2;y3;y4];


Mt_Est_Px=zeros（MN）;
Mt_Est_Py=zeros（MN）;

%?nx=randn（NM）*Delta;
%?ny=randn（NM）*Delta;
%?Zx=Rx*ones（1M）+nx;
%?Zy=Ry*ones（1M）+ny;

Zx=Rx*ones（1M）;
Zy=Ry*ones（1M）;

for?m=1:M
????Mt_Est_Px（m1）=Zx（1m）;
????Mt_Est_Py（m1）=Zx（2m）;
????xn（1）=Zx（1m）;
????xn（2）=Zx（2m）;
????yn（1）=Zy（1m）;
????yn（2）=Zy（2m）;
????phi=[1T00;0100;001T;0001];
????h=[1000;0010];
????g=[T/20;10;0T/2;01];
????q=[Delta^20;0Delta^2];
????vx=（Zx（2）-Zx（1m））/2;
????vy=（Zy（2）-Zy（1m））/2;
????x_est=[Zx（2m）;vx;Zy（2m）;vy];
????p_est=[Delta^2Delta^2/T00;Delta^2/T2*Delta^2/（T^2）00;
????????00Delta^2Delta^2/T;00Delta^2/T2*Delta^2/（T^2）];
????Mt_Est_Px（m2）=x_est（1）;
????Mt_Est_Py（m2）=x_est（3）;
????for?r=3:N
????????z=[Zx（rm）;Zy（rm）];
????????if?r<20
????????????x_pre=phi*x_est;
????????????p_pre=phi*p_est*phi‘;
????????????k=p_pre*h‘*inv（h*p_pre*h‘+q）;
????????????x_est=x_pre+k*（z-h*x_pre）;
????????????p_est=（eye（4）-k*h）*p_pre;
????????????xn（r）=x_est（1）;
????????????yn（r）=x_est（3）;
????????????Mt_Est_Px（mr）=x_est（1）;
????????????Mt_Est_Py（mr）=x_est（3）;
????????else
????????????if?r==20
????????????????X_est=[x_est;0;0];
????????????????P_est=p_est;
????????????????P_est（66）=0;
????????????????for?i=1:3
????????????????????Xn_est{i1}=X_est;
????????????????????Pn_est{i1}=P_est;
????????????????end
????????????????u=[0.80.10.1];
????????????end
????????????[X_estP_estXn_estPn_estu]=IMM（Xn_estPn_estTzDeltau）;
????????????xn（r）=X_est（1）;
?????????

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

?????文件????8223212??2015-11-21?17:50??29、基于卡爾曼濾波的GPS數據處理研究\BaiDuMapDemo.rar

?????文件??????60891??2015-11-21?17:49??29、基于卡爾曼濾波的GPS數據處理研究\BaiDuMap_Winform.rar

?????文件????3602352??2017-03-29?21:59??29、基于卡爾曼濾波的GPS數據處理研究\參考文獻\GPS定位算法及其在智能公交中的應用.pdf

?????文件????1849515??2017-03-29?16:33??29、基于卡爾曼濾波的GPS數據處理研究\參考文獻\一種基于GPS采集處理系統的研究_馬健.caj

?????文件?????669184??2017-03-27?09:24??29、基于卡爾曼濾波的GPS數據處理研究\參考文獻\基于卡爾曼濾波的目標跟蹤研究.doc

?????文件????1164370??2017-03-29?19:16??29、基于卡爾曼濾波的GPS數據處理研究\參考文獻\基于嵌入式的農機輔助導航系統研究_于占寶.caj

?????文件????4227164??2017-03-29?16:21??29、基于卡爾曼濾波的GPS數據處理研究\參考文獻\嵌入式農機GPS導航及變量施肥控制技術的研究_王鵬.caj

?????文件???13069312??2017-06-29?15:11??29、基于卡爾曼濾波的GPS數據處理研究\基于卡爾曼濾波的GPS數據處理研究.doc

?????文件????????314??2017-06-29?16:47??29、基于卡爾曼濾波的GPS數據處理研究\基于卡爾曼濾波的GPS數據處理研究.files\colorschememapping.xml

?????文件???????9012??2017-06-29?16:47??29、基于卡爾曼濾波的GPS數據處理研究\基于卡爾曼濾波的GPS數據處理研究.files\filelist.xml

?????文件???????5120??2017-06-29?16:47??29、基于卡爾曼濾波的GPS數據處理研究\基于卡爾曼濾波的GPS數據處理研究.files\header.htm

?????文件????????645??2017-06-29?16:47??29、基于卡爾曼濾波的GPS數據處理研究\基于卡爾曼濾波的GPS數據處理研究.files\image001.wmz

?????文件???????1064??2017-06-29?16:47??29、基于卡爾曼濾波的GPS數據處理研究\基于卡爾曼濾波的GPS數據處理研究.files\image002.gif

?????文件????????511??2017-06-29?16:47??29、基于卡爾曼濾波的GPS數據處理研究\基于卡爾曼濾波的GPS數據處理研究.files\image003.wmz

?????文件????????200??2017-06-29?16:47??29、基于卡爾曼濾波的GPS數據處理研究\基于卡爾曼濾波的GPS數據處理研究.files\image004.gif

?????文件????????474??2017-06-29?16:47??29、基于卡爾曼濾波的GPS數據處理研究\基于卡爾曼濾波的GPS數據處理研究.files\image005.wmz

?????文件????????199??2017-06-29?16:47??29、基于卡爾曼濾波的GPS數據處理研究\基于卡爾曼濾波的GPS數據處理研究.files\image006.gif

?????文件????????471??2017-06-29?16:47??29、基于卡爾曼濾波的GPS數據處理研究\基于卡爾曼濾波的GPS數據處理研究.files\image007.wmz

?????文件????????139??2017-06-29?16:47??29、基于卡爾曼濾波的GPS數據處理研究\基于卡爾曼濾波的GPS數據處理研究.files\image008.gif

?????文件????????471??2017-06-29?16:47??29、基于卡爾曼濾波的GPS數據處理研究\基于卡爾曼濾波的GPS數據處理研究.files\image009.wmz

?????文件????????214??2017-06-29?16:47??29、基于卡爾曼濾波的GPS數據處理研究\基于卡爾曼濾波的GPS數據處理研究.files\image010.gif

?????文件????????471??2017-06-29?16:47??29、基于卡爾曼濾波的GPS數據處理研究\基于卡爾曼濾波的GPS數據處理研究.files\image011.wmz

?????文件????????199??2017-06-29?16:47??29、基于卡爾曼濾波的GPS數據處理研究\基于卡爾曼濾波的GPS數據處理研究.files\image012.gif

?????文件????????560??2017-06-29?16:47??29、基于卡爾曼濾波的GPS數據處理研究\基于卡爾曼濾波的GPS數據處理研究.files\image013.wmz

?????文件????????342??2017-06-29?16:47??29、基于卡爾曼濾波的GPS數據處理研究\基于卡爾曼濾波的GPS數據處理研究.files\image014.gif

?????文件????????524??2017-06-29?16:47??29、基于卡爾曼濾波的GPS數據處理研究\基于卡爾曼濾波的GPS數據處理研究.files\image015.wmz

?????文件????????216??2017-06-29?16:47??29、基于卡爾曼濾波的GPS數據處理研究\基于卡爾曼濾波的GPS數據處理研究.files\image016.gif

?????文件????????513??2017-06-29?16:47??29、基于卡爾曼濾波的GPS數據處理研究\基于卡爾曼濾波的GPS數據處理研究.files\image017.wmz

?????文件????????545??2017-06-29?16:47??29、基于卡爾曼濾波的GPS數據處理研究\基于卡爾曼濾波的GPS數據處理研究.files\image018.wmz

?????文件????????311??2017-06-29?16:47??29、基于卡爾曼濾波的GPS數據處理研究\基于卡爾曼濾波的GPS數據處理研究.files\image019.gif

............此處省略289個文件信息