GNSS/INS组合导航学习-GINAV（一）

组合导航算法详解

原创

已于 2022-12-17 13:22:21 修改 · 9.9k 阅读

94 ·

CC 4.0 BY-SA版权

文章标签：

#学习

于 2022-12-15 20:35:26 首次发布

本文详细介绍了一种组合导航算法，包括严老师PSINS工具箱、GINAV及KF-GINS等开源程序，并深入解析了反对称矩阵、方向余弦矩阵转换、地球相关参数计算等核心概念和技术细节。

从今天开始整理一下，最近半年学习的组合导航算法

目前开源程序

1、严老师PSINS工具箱（MATLAB——卡尔曼滤波算法)

2、GINAV （MATLAB——卡尔曼滤波算法）

3、KF-GINS （C++——卡尔曼滤波算法）

当然也有图优化算法，后续有时间会继续整理的。参考书：严老师的捷联惯导与卡尔曼滤波原理、武汉大学牛老师组合导航算法讲义，分别对以上三个程序详细整理。

注意：采取的坐标系不同，捷联惯导程序也会有所不同。（东北天与北东地）

GINAV

分别有以下12个文件夹

今天主要整理ins文件夹

INS_1——askew.m

function matrix = askew(vector)
% convert 3x1 vector to 3x3 askew matrix.

matrix = [        0,  -vector(3),   vector(2);
          vector(3),           0,  -vector(1);
         -vector(2),   vector(1),           0];
return

这个是最常见的反对称矩阵

知识点：反对称矩阵是正规矩阵、反对称的幂方通式、反对称的矩阵指数函数

matrix 3*3的矩阵 vector是 3*1的矩阵

INS_2——att2Cnb.m

function Cnb = att2Cnb(att)

sina=sin(att); cosa=cos(att);
sinp=sina(1);  sinr=sina(2); siny=sina(3);
cosp=cosa(1);  cosr=cosa(2); cosy=cosa(3);
Cnb=[ cosr*cosy-sinp*sinr*siny, -cosp*siny,  sinr*cosy+sinp*cosr*siny;
      cosr*siny+sinp*sinr*cosy,  cosp*cosy,  sinr*siny-sinp*cosr*cosy;
                    -cosp*sinr,       sinp,                 cosp*cosr];

return

将欧拉角转换为方向余弦

航向角y：运载体纵轴在当地水平面上的投影线与当地地理北向的夹角，常取北偏东为正角度范围为0～360°

俯仰角p：运载体纵轴与其水平投影线之间的夹角，当运载体抬头时角度定义为正，角度范围-90°～90°，

横滚角r：运载体立轴与纵轴所在铅垂面之间的夹角，当运载体向右倾斜时角度定义为正，角度范围-180°～180°

INS_3——Cnb2att.m

function att = Cnb2att(Cnb)

att=[asin(Cnb(3,2)); atan2(-Cnb(3,1),Cnb(3,3)); atan2(-Cnb(1,2),Cnb(2,2))];
    
return

从方向余弦到欧拉角（此程序没有考虑奇异点、但是在严老师的PSINS中考虑了）

INS_4——earth_update.m

function eth=earth_update(pos,vel)
eth.Re = 6378137;                    
eth.f  = 1/298.257223563;            
eth.Rp = (1-eth.f)*eth.Re;           
eth.e1 = sqrt(eth.Re^2-eth.Rp^2)/eth.Re;
eth.e2 = sqrt(eth.Re^2-eth.Rp^2)/eth.Rp; 
eth.wie = 7.2921151467e-5;           
eth.g0 = 9.7803267714;

以上是WGS84椭球参数

1、长半轴a = 6378137.0 m、短半轴b=6356752.3142m、扁率f =1/298.257223563;长半轴a = 6378137.0 m、短半轴b=6356752.3142m、扁率f =1/298.257223563、第─偏心率平方(e2)= 0.00669437999013、第二偏心率平方(e 2)= 0.00673949674222。

2、地球自转角速度= 7.292115×10-5 rad/s、椭球正常重力位= 62636860.8497 m2/s2、赤道正常重力=9.7