惯性传感器下的运动轨迹生成

这是借助于惯性传感器的一个轨迹matlab仿真程序，读入的是实验测得的处理后的txt文本，，文件1 2 3 静止时测量的惯导初始位置和姿态。文件4 5 6 旋转时测量的惯性导航对震动的敏感测量。

 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个文件信息