基于matlab的捷联惯导算法编程（三）

最新推荐文章于 2022-07-06 19:48:06 发布

Kye..

最新推荐文章于 2022-07-06 19:48:06 发布

阅读量6k

点赞数 5

本文链接：https://blog.csdn.net/m0_51774116/article/details/117251168

版权

本文详细介绍了基于MATLAB的捷联惯导算法主程序编程，包括整体代码和分步解析：定义变量、设置时间及加载IMU数据、记录导航参数、筛选AVP数据、时间比对以及坐标和角度转换。通过这些步骤，实现对IMU数据的处理和导航参数计算。

摘要由CSDN通过智能技术生成

前言

接续基于matlab的捷联惯导算法编程（二），对主程序进行编程，实现运算。

（二）捷联惯导算法主程序

1.整体代码

%捷联惯导算法仿真主程序 
clear
clc
format long
gvar; %加载全局变量
nn=2;    %子样数，采用二样子圆锥误差补偿算法
ts=0.01;  %单个采样间隔的采样时间长度
nts=nn*ts; %采样周期，在nts时间内进行两次采样，每次采样 0.1秒
%输入姿态角向量att含三个分量，依次为俯仰角θ(Theta)，横滚角γ(Gama)，航向角ψ(Fai)
att=[-1.42701;0.569073;-176.015]*arcdeg;    
vn=[0;0;0];  %初始化速度
%初始位置 纬度（B）34.8222007度 ,经度（L）113.4845389度 ，高度（h）87.2566m   B L H相当于大地坐标系的纬度、经度、高度
pos=[34.8222007*arcdeg;113.4845389*arcdeg;87.2566]; 
qnb=a2qua(att);% 姿态、速度和位置初始化  ；姿态角转换为四元数  b->n
%仿真静态IMU数据
eth=earth(pos,vn); %地球导航参数参数计算
%qconj 四元数共轭，地球自转角速度由 导航系换算到机体系，作为机体角速度，并乘时间得到角度增量
wm=qmulv(qconj(qnb),eth.wnie)*ts;  
%比力由 导航系换算到机体系，作为机体比力，并乘时间得速度增量
vm=qmulv(qconj(qnb),-eth.gn)*ts;   
wm=repmat(wm',nn,1); %repmat 重复数组副本。转置角速度矢量，并复制到nn行'1'列 相当于二子样
vm=repmat(vm',nn,1); %转置速度矢量，并复制得到nn行'1'列
%仿真时间   
phi=[0.1;0.2;3]*arcmin;  %由计算四元数和真实四元数计算失准角误差  phi
qnb=qaddphi(qnb,phi);    %四元数加失准角误差 转换为qnb           
len=fix(1550/ts);        %仿真时长，得到fix(1550/ts)个子样 fix(1550/ts)
avp=zeros(len,10);  %由零组成的len*10的向量
kk=1;  
t=0; 
a=load('imu_t.imu'); %加载imu数据
b=a(:,2:4)*arcdeg*ts;
c=a(:,5:7)*ts;
a1=load('pos_t .pos'); %加载.pos数据  
len=311286; 
%记录导航结果[att,vn,pos,t]  共有len个子样，每个采样时间段nn个子样，共len/nn个时间段，循环len/nn步                               
for k=36000:nn:len                                  %k是两次一跳，因为采用的是二子样，36000是6分钟开始解算
    t=t+nts;                                        %t表示采样时间段
    [qnb,vn,pos]=insupdate(qnb,vn,pos,wm,vm,ts);    
    avp(kk,:)=[q2att(qnb);vn;pos;t]';               %avp()=[θ γ ψ VE VN VU B L h t]
    kk=kk+1;    
    if mod(t,100)<nts,  disp(fix(t));  
    end                                             %显示进度  disp函数：显示文本或数组
   wm=b(k:k+1,:);
   vm=c(k:k+1,:);
end
%%%%%%%%%%%%%%
%取出avp中只有1s时的数据
%%%%%%%%%%%%%%
avp1=zeros(3000,10);
avp(:,10)=avp(:,10)*100;
k1=1;
for i=1:length(avp)
    c=mod(avp(i,10),100);
    d=100-c;
    if(c<0.0000000001||d<=0.000001)
        av