?clear?;
close?all;
%--------------------------------------------------------------------------
%******************************KF跟蹤算法仿真*******************************
%--------------------------------------------------------------------------

T=0.1;
rat=100;
atti_rat=200;
data_num=2000;
data_num1=2000;
N=data_num/10;
N1=data_num1/10;
R=zeros（66N）;%生成6*6的0矩陣，共N個，N=200
RR=zeros（66N）;
F=[1?0?0?T?0?0?;
???0?1?0?0?T?0?;
???0?0?1?0?0?T?;
???0?0?0?1?0?0?;
???0?0?0?0?1?0?;
???0?0?0?0?0?1];
?J=[1?0?0?0?0?0;
????0?1?0?0?0?0;
????0?0?1?0?0?0;
????0?0?0?1?0?0;
????0?0?0?0?1?0;
????0?0?0?0?0?1];
G=[1/2*T^2?0?0;
????0?1/2*T^2?0;
????0?0?1/2*T^2;
????T????0??0;
????0????T??0;
????0????0??T];

x=zeros（61N+1）;%狀態真實值X（k）
xcan=zeros（61N+1）;%狀態真實值X（k）

%%%%%%%%%%錨節點坐標%%%%%%%%%%%%%%%%%%%%%%%%%
%初始化?X（1）
?x（::1）=[0?0?0?0.5?0?0]‘;
xcan（::1）=[0?0?0?0.5?0?0]‘;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%以下是路徑產生%%%%%%%%%%%%%%%%%%%
fid1=fopen（‘實驗測試數據\1.txt‘‘r‘）;%打開文件，并讀
[titlecount]=fscanf（fid1‘%6x‘[6data_num1]）;%打開文件讀取數據，6行data_num1列
tt1=title‘;
status=fclose（fid1）;
fid2=fopen（‘實驗測試數據\5.txt‘‘r‘）;
[titlecount]=fscanf（fid2‘%6x‘[6data_num]）;
tt2=title‘;
status=fclose（fid2）;
%%%%%%%%%%%%%%%%%%%%%%%%%%%參考數據處理%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
acex1=tt1（:4）;%y=x（:i）的作用就就是把矩陣x的第i列取出來賦值到y里面，那么得到的y就是一個列向量
acey1=tt1（:5）;
acez1=tt1（:6）;
attix1=tt1（:1）;
attiy1=tt1（:2）;
attiz1=tt1（:3）;
for?i=1:data_num1
????if?acex1（i）>1000
????????acex1（i）=acex1（i）-16383;
????end
????if?acey1（i）>1000
????????acey1（i）=acey1（i）-16383;
????end
????if?acez1（i）>1000
????????acez1（i）=acez1（i）-16383;
????end
????if?attix1（i）>1000
????????attix1（i）=attix1（i）-16383;
????end
????if?attiy1（i）>1000
????????attiy1（i）=attiy1（i）-16383;
????end
????if?attiz1（i）>1000
????????attiz1（i）=attiz1（i）-16383;
????end
????if?mod（i10）==0%求余函數
????????acex11（floor（i/10））=acex1（i）;
????????acey11（floor（i/10））=acey1（i）;
????????acez11（floor（i/10））=acez1（i）;
????????attix11（floor（i/10））=attix1（i）;
????????attiy11（floor（i/10））=attiy1（i）;
????????attiz11（floor（i/10））=attiz1（i）;
????end
end
acexmean1=mean（acex11）/rat;%平均數
aceymean1=mean（acey11）/rat;
acezmean1=mean（acez11）/rat;
attixmean1=mean（attix11）/atti_rat;
attiymean1=mean（attiy11）/atti_rat;
attizmean1=mean（attiz11）/atti_rat;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%姿態矩陣
%?xx=attixmean1;
%?yy=attiymean1;
%?zz=attizmean1;
%?
%?Cbn=[cos（zz）*cos（xx）+sin（zz）*sin（xx）*sin（yy）?-cos（zz）*sin（xx）+sin（zz）*cos（xx）*sin（yy）?-sin（zz）*cos（yy）;
%????????sin（xx）*cos（yy）?????????????????????????????cos（xx）*cos（yy）??????????????????????sin（yy）???????;
%??????sin（zz）*cos（xx）-cos（zz）*sin（xx）*sin（yy）?-sin（zz）*sin（xx）-cos（zz）*cos（xx）*sin（yy）?cos（zz）*cos（yy）];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%參考數據處理完畢%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%測試數據處理%%%%%%%%%%%%%%%%%%%%%%
acex2=tt2（:4）;
ace

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

?????文件??????15751??2018-03-15?10:15??軌跡生成\INSshiyan6.m

?????文件????????209??2015-03-30?10:11??軌跡生成\location_create.m

?????文件????????169??2018-03-14?17:09??軌跡生成\Untitled.m

?????文件??????74232??2015-03-30?10:11??軌跡生成\實驗測試數據\1.TXT

?????文件?????226630??2015-03-30?10:11??軌跡生成\實驗測試數據\10.TXT

?????文件??????74232??2015-03-30?10:11??軌跡生成\實驗測試數據\2.TXT

?????文件??????74232??2015-03-30?10:11??軌跡生成\實驗測試數據\3.TXT

?????文件??????74232??2015-03-30?10:11??軌跡生成\實驗測試數據\4.TXT

?????文件??????74232??2015-03-30?10:11??軌跡生成\實驗測試數據\5.TXT

?????文件??????74232??2015-03-30?10:11??軌跡生成\實驗測試數據\6.TXT

?????文件?????222195??2015-03-30?10:11??軌跡生成\實驗測試數據\7.TXT

?????文件?????219852??2015-03-30?10:11??軌跡生成\實驗測試數據\8.TXT

?????文件?????226630??2015-03-30?10:11??軌跡生成\實驗測試數據\9.TXT

?????文件????????368??2015-03-30?10:11??軌跡生成\實驗測試數據\數據說明.txt

?????文件??????75506??2014-01-13?11:18??軌跡生成\實驗測試數據\源文件\SAVE2014-01-13_11-18-41.TXT

?????文件??????75566??2014-01-13?11:19??軌跡生成\實驗測試數據\源文件\SAVE2014-01-13_11-19-24.TXT

?????文件??????75626??2014-01-13?11:20??軌跡生成\實驗測試數據\源文件\SAVE2014-01-13_11-20-03.TXT

?????文件??????75626??2014-01-13?11:27??軌跡生成\實驗測試數據\源文件\SAVE2014-01-13_11-27-17.TXT

?????文件??????75626??2014-01-13?11:27??軌跡生成\實驗測試數據\源文件\SAVE2014-01-13_11-27-50.TXT

?????文件??????75567??2014-01-13?11:28??軌跡生成\實驗測試數據\源文件\SAVE2014-01-13_11-28-28.TXT

?????文件?????226578??2014-01-13?11:30??軌跡生成\實驗測試數據\源文件\SAVE2014-01-13_11-30-10.TXT

?????文件?????224323??2014-01-13?11:41??軌跡生成\實驗測試數據\源文件\SAVE2014-01-13_11-41-28.TXT

?????文件?????226630??2014-01-13?11:52??軌跡生成\實驗測試數據\源文件\SAVE2014-01-13_11-52-47.TXT

?????文件?????226630??2014-01-13?12:12??軌跡生成\實驗測試數據\源文件\SAVE2014-01-13_12-12-37.TXT

?????文件?????283327??2015-03-30?10:11??軌跡生成\實驗測試數據\源文件.rar

?????目錄??????????0??2018-03-04?15:09??軌跡生成\實驗測試數據\源文件

?????目錄??????????0??2018-03-04?15:09??軌跡生成\實驗測試數據

?????目錄??????????0??2018-03-14?17:10??軌跡生成

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

??????????????2998201????????????????????28

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