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

目录

前言

（一）matlab子函数

1.全局变量（gvar）

2.三维向量的反对称阵

3.姿态角转换为姿态阵

4.姿态阵转换为姿态角

5.姿态角转换为四元数

6.四元数转换为姿态角

7.姿态阵转换为四元数

8.四元数转换为姿态阵

9.旋转矢量转换为变换矩阵

10.旋转矢量转换为变换四元数

11.变换四元数转换为旋转矢量

12.四元数共轭

13.四元数归一化

14.四元数相乘

15.四元数乘向量

16.四元数加失准角误差

17.四元数减失准角

18.由计算四元数和真实四元数计算失准角误差

19.圆锥/划船误差补偿

20.地球导航参数计算

21.捷联惯导更新算法

22.辅助函数——求位置增量

23.辅助函数——作图

24.大地转直角坐标

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

1.整体代码

2.分步代码

（1）定义变量

（2）设置时间和加载imu

（3）记录导航参数结果

（4）对avp数据的导航参数结果进行筛选

（5）时间比对

（6）avp中的[pos]及att转换

---

前言

捷联惯导算法，参考严恭敏老师的代码，其中p245，代表严恭敏老师编写的《捷联惯导和组合导航》书中的解释，而后本人进行了稍微的改编，插入了自己的imu数据和组合导航的.pos数据，该算法是按照程序设计的模块化原则，分解为一系列的子函数模块。首先，本文逐个给出各matlab子函数程序，其次，利用各子函数编写捷联惯导算法主程序，构成了算法具体实现的一个框架。最后，载入imu数据中的角速度增量和速度增量与组合导航结算的.pos参考文件进行作图对比。

imu数据：(5条消息) LooselyCoupled-Reference.pos-其它文档类资源-优快云下载

组合导航.pos数据：(5条消息) IMU-Measurements._imu-其它文档类资源-优快云下载

惯性导航解算指导：(2条消息) 惯性导航解算ppt.pdf-其它文档类资源-优快云下载

惯性导航解算原理公式：(2条消息) 惯性导航解算原理公式.pdf-其它文档类资源-优快云下载

捷联惯导matlab程序：(2条消息) 捷联惯导MATLAB程序.pdf-其它文档类资源-优快云下载

（一）matlab子函数

1.全局变量（gvar）

%全局变量(gvar)
global GM Re ff wie ge gp ug arcdeg arcmin arcsec hur dph dpsh ugpsHz  %定义全局变量
GM=3.986004415e14;      %引力
Re=6.378136998405e6;    %赤道长半轴
wie= 7.2921151467e-5;   %自传角速度 wgs-84 model
ff=1/298.257223563;     %扁率
ee=sqrt(2*ff-ff^2);     %扁心率
e2=ee^2;
Rp=(1-ff)*Re;           %极轴半径
ge=9.780325333434361;   %ge 赤道重力加速度
gp=9.832184935381024;   %gp 极点重力加速度
g0=9.780325333434361;   %ug 微重力加速度
ug=ge*1e-6; 
arcdeg=pi/180;         %弧度=角度×π÷180°
arcmin=arcdeg/60;
arcsec=arcmin/60;      %angle unit .acdeg为弧度
hur=3600;
dph=arcdeg/hur;
dpsh=arcdeg/sqrt(hur); %hour;deg/hour;deg/sqrt(hour)
ugpsHz= ug/sqrt(1);    %ug/sqrt(Hz)

2.三维向量的反对称阵

%反对称阵,斜对称阵
function m=askew(v)
m=[ 0, -v(3), v(2);
    v(3), 0, -v(1);
    -v(2), v(1), 0 ];

3.姿态角转换为姿态阵

%姿态角转换为姿态阵 参考p245, B.3式
function Cnb= a2mat(att)
s=sin(att);c=cos(att);
si=s(1);sj=s(2);sk=s(3);
ci=c(1);cj=c(2);ck=c(3);
% %输入姿态角向量att含三个分量，
% 依次为俯仰角θ(Theta)，横滚角γ(Goma)，航向角ψ(Fai)，
% 特别注意：程序中定位方位角北偏西为正，取值范围(-pi,pi].
%s(1)=sinθ,s(2)=sinγ,s(3)=sinψ； 
%c(1)=cosθ,c(2)=cosγ,c(3)=cosψ；
Cnb=[ cj*ck-si*sj*sk, -ci*sk, sj*ck+si+cj*sk;
      cj*sk+si*sj*sk,  ci*ck, sj*ck-si*cj*ck;
      -ci*sj,          si,    ci*cj ];
  

4.姿态阵转换为姿态角

%姿态阵转换为姿态角 参考p245, B.4-B.7
function att=m2att(Cnb)
%当俯仰角在+-pi/2附近时，横滚角和航向角之间是无法单独分离的，不能计算出来。
if abs(Cnb(3,2))<=0.999999
    att=[asin(Cnb(3,2)); -atan2(Cnb(3,1),Cnb(3,3)); -atan2(Cnb(1,2),Cnb(2,2)) ];
else
    att=[asin(Cnb(3,2)); atan2(Cnb(1,3), Cnb(1,1)); 0 ];
end

5.姿态角转换为四元数

%姿态角转换为四元数，参考p247, B.12
function qnb =a2qua(att)
s=sin(att/2);c=cos(att/2);
si=s(1);sj=s(2);sk=s(3);
ci=c(1);cj=c(2);ck=c(3);
qnb=[ ci*cj*ck - si*sj*sk;
      si*cj*ck - ci*sj*sk;
      ci*sj*ck + si*cj*sk;
      ci*cj*sk + si*sj*ck ];
%该转换也可通过姿态阵作为中间变量，先将姿态角转换为姿态阵再转换为四元数
%qnb=m2qua(a2mat(att));

6.四元数转换为姿态角

%四元数转换为姿态角 
function att=q2att(qnb)
%四元数->姿态阵->姿态角
att=m2att(q2mat(qnb));

7.姿态阵转换为四元数

%姿态阵转为四元数 p247 . B.11
function qnb =m2qua(Cnb)
C11=Cnb(1,1);C12=Cnb(1,2);C13=Cnb(1,3);
C21=Cnb(2,1);C22=Cnb(2,2);C23=Cnb(2,3);
C31=Cnb(3,1);C32=Cnb(3,2);C33=Cnb(3,3);
if C11>=C22+C33
    q1=0.5*sqrt(1+C11-C22-C3