十八与她的博客

该部分的核心代码主要是实现组合导航的核心算法即Kalman滤波，其中表示Kalman滤波的参数初始化，表示创建连续状态转移矩阵，表示创建离散时间状态转移矩阵，表示Kalman滤波的时间更新和量测更新部分，其中包含Sage-Husa自适应滤波、抗差Kalman滤波等。

惯性导航初始对准包括间接粗对准和卡尔曼滤波精对准，其中。为基于卡尔曼滤波的精对准，其中。

为惯导姿态更新中利用旋转矢量更新四元数的函数，其余函数见函数备注及符号。表示姿态四元数转换为方向余弦矩阵，表示姿态四元数转换为等效旋转矢量，表示计算两个四元数所对应的失准角，表示计算一个向量的反对称矩阵，表示姿态四元数转换为欧拉角，文件全部都展示如下，其中。

由解算时间段内比力方向在空间旋转变化引起的误差。

19世纪50年代是机械陀螺仪飞速发展的一个重要时期,也正是在那时发现了著名的圆锥运动现象，即当陀螺仪在其旋转轴和输出轴出现同频不同相的角振动时,尽管其测量输入轴净指向不变(从整体上看没有随时间改变的趋势项),但陀螺仪在输入轴上还是会敏感到并输出常值角速率信号，而由圆锥运动所引起的误差称为圆锥误差。

四元数可用于描述刚体转动或姿态变换，与方向余弦阵相比，表达简洁。四元数是一个包含四个元素的列向量，列向量的第一个元素表示刚体围绕旋转轴转过的角度大小，其余3个元素代表了旋转轴的方向。

量化噪声是一切量化操作所固有的噪声，只要进行数字量化编码采样，传感器输出的理想值与量化值之间就必然会存在微小的差别，量化噪声代表了传感器检测的最小分辨率水平。角度随机游走是宽带角速率白噪声积分的结果，即陀螺从零时刻起累积的总角增量误差表现为随机游走，而每一时刻的等效角速率误差表现为白噪声。根据随机过程理论，随机游走是一种独立增量过程，对于陀螺角度随机游走而言“独立增量”的含义便是:角速率白噪声在两相邻采样时刻进行积分（增量)，不同时间段的积分值之间互不相关（独立）。

在惯导系统进行粗对准之后，惯导平台和当地导航坐标系之间还存在一定偏差，此时基于粗对准的结果，利用以速度为观测量的Kalman滤波对姿态进行估计。

间接粗对准方法具有更好的角晃动抗干扰能力，只是在短时间内线晃动抗干扰能力相对弱些，若适当延长粗对准时间（一般在分钟量级），间接粗对准方法往往都能够取得良好的效果。间接粗对准方法是一种非常实用的捷联惯导粗对准方法，得到了广泛的应用。在惯导系统进入工作前，需要进行初始对准，使得惯导系统与当地导航坐标系指向一致，便于惯导系统更好地进入工作状态。博主利用C语言实现了间接粗对准，间接粗对准较之解析粗对准更有优势。以上函数中使用的其他函数块，已在之前或之后的博客中给出。

在进行惯导机械编排（惯性导航算法）之前，需要对惯导系统的初始位置、姿态和速度进行初始化；以及姿态四元数、方向余弦矩阵、时间不同步及空间杆臂等参数，因此也需将这一部分模块化，方便程序调用。

在分析时间不同步误差时，假设惯导与卫导之间的杆臂误差已经得到校正。

不同导航系统的导航信息进行比对和融合，必须对导航信息实施转换，转换至统一的参考基准下表示。

我们知道一般情况下，GNSS观测值得更新频率是低于惯导数据得输出频率的，而组合导航计算结果的输出频率取决于惯导的数据频率以及所使用的姿态优化方法（单子样、双子样等）。对于组合导航的参考值来说，其更新频率也低于惯导和组合导航结果，因此，若要将组合导航结果与参考值作比较，进而求出误差等一系列统计数据，就需要对计算的组合导航结果进行插值，插值到和参考值相同的频率，即使两者的时间对应相同。

欧拉证明任意两个正交坐标系之间的相对朝向关系可以通过一组不少于3的角度来描述。这三次旋转的转角称为一组欧拉角，因最早由欧拉(Leonhard Euler )提出而得名。在三维欧氏空间里，任何两个正交坐标系都可以用坐标变换把它们联系起来，而坐标变换又可以用坐标旋转来得到。一个动坐标系相对参考坐标系的方位，可以完全由动坐标系依次绕三个不同的转轴的转角来确定。方向余弦矩阵(Direction Cosine Matrix，DCM）又被称为“坐标转换矩阵”，常用于将矢量的投影从一个坐标系变换到另一坐标系中。

在进行惯导的机械编排时，需要在每个时刻都进行地球自转参数的更新，比如哥氏加速度，有害加速度等，因此将其整理成一个函数块。

【代码】C语言实现矩阵求逆（可抵制奇异值、坏值导致的奇异解）

在做组合导航的时候，需要用到正反向解算，在将正反结算结果进行融合的时候，需要将反算结果倒过来，变成正序的再进行融合，使用函数比较方便，因此本文给出自编的一个小函数。

力学中刚体的有限次转动是不可交换的。转动的不可交换性决定了转动不是矢量，即两次以上的不同轴转动不能相加。对一个空间方向随时间变化的角速度矢量进行积分是没有物理意义的。而旋转矢量破解了这个难题。一个坐标系到另一个坐标系的变换可以通过多次转动来完成，也可以通过绕一个定义在参考坐标系中的矢量的单次转动来实现。该矢量称作等效旋转矢量(rotation vector）是一个三元素的向量，旋转矢量的方向给出了转动轴的方向，它的模长为转动角度的大小。又称为轴角（axis-angle)。

在将三维矢量代数推广至乘法和除法运算的研究中，爱尔兰数学家、物理学家哈密顿于1843年创建了四元数（(quaternion)和四元数代数。四元数是指由一个实数单位1和三个虚数单位i，j，k组成并具有下列形式实元的数。单位1，i，j，k可以看作四维空间（用符号H来表示)中的一组单位矢量。任何四元数都可看作该空间中的一个点或向量。

力学中刚体的有限次转动是不可交换的。一个坐标系到另一个坐标系的变换可以通过多次转动来完成，也可以通过绕一个定义在参考坐标系中的矢量的单次转动来实现。该矢量称作等效旋转矢量(rotation vector）是一个三元素的向量，旋转矢量的方向给出了转动轴的方向，它的模长为转动角度的大小。单位1，i，j，k可以看作四维空间（用符号H来表示)中的一组单位矢量。方向余弦矩阵(Direction Cosine Matrix，DCM）又被称为“坐标转换矩阵”，常用于将矢量的投影从一个坐标系变换到另一坐标系中。

欧拉证明任意两个正交坐标系之间的相对朝向关系可以通过一组不少于3的角度来描述。在三维欧氏空间里，任何两个正交坐标系都可以用坐标变换把它们联系起来，而坐标变换又可以用坐标旋转来得到。一个动坐标系相对参考坐标系的方位，可以完全由动坐标系依次绕三个不同的转轴的转角来确定。四元数是指由一个实数单位1和三个虚数单位i，j，k组成并具有下列形式实元的数。单位1，i，j，k可以看作四维空间（用符号H来表示)中的一组单位矢量。失准角度可以简单理解为，计算的导航坐标系与理想导航坐标系之间的差别，用角度来表示。

力学中刚体的有限次转动是不可交换的。转动的不可交换性决定了转动不是矢量，即两次以上的不同轴转动不能相加。一个坐标系到另一个坐标系的变换可以通过多次转动来完成，也可以通过绕一个定义在参考坐标系中的矢量的单次转动来实现。该矢量称作等效旋转矢量(rotation vector）是一个三元素的向量，旋转矢量的方向给出了转动轴的方向，它的模长为转动角度的大小。方向余弦矩阵(Direction Cosine Matrix，DCM）又被称为“坐标转换矩阵”，常用于将矢量的投影从一个坐标系变换到另一坐标系中。

在三维欧氏空间里，任何两个正交坐标系都可以用坐标变换把它们联系起来，而坐标变换又可以用坐标旋转来得到。一个动坐标系相对参考坐标系的方位，可以完全由动坐标系依次绕三个不同的转轴的转角来确定。姿态角就属于欧拉角。四元数是指由一个实数单位1和三个虚数单位i，j，k组成并具有下列形式实元的数。单位1，i，j，k可以看作四维空间（用符号H来表示)中的一组单位矢量。方向余弦矩阵(Direction Cosine Matrix，DCM）又被称为“坐标转换矩阵”，常用于将矢量的投影从一个坐标系变换到另一坐标系中。

在将三维矢量代数推广至乘法和除法运算的研究中，爱尔兰数学家、物理学家哈密顿于1843年创建了四元数（(quaternion)和四元数代数。四元数是指由一个实数单位1和三个虚数单位i，j，k组成并具有下列形式实元的数。单位1，i，j，k可以看作四维空间（用符号H来表示)中的一组单位矢量。任何四元数都可看作该空间中的一个点或向量。失准角度可以简单理解为，计算的导航坐标系与理想导航坐标系之间的差别，用角度来表示。具体专业理解可参考相应书籍。

力学中刚体的有限次转动是不可交换的。转动的不可交换性决定了转动不是矢量，即两次以上的不同轴转动不能相加。一个坐标系到另一个坐标系的变换可以通过多次转动来完成，也可以通过绕一个定义在参考坐标系中的矢量的单次转动来实现。该矢量称作等效旋转矢量(rotation vector）是一个三元素的向量，旋转矢量的方向给出了转动轴的方向，它的模长为转动角度的大小。四元数是指由一个实数单位1和三个虚数单位i，j，k组成并具有下列形式实元的数。单位1，i， j，k可以看作四维空间（用符号H来表示)中的一组单位矢量。

在三维欧氏空间里，任何两个正交坐标系都可以用坐标变换把它们联系起来，而坐标变换又可以用坐标旋转来得到。一个动坐标系相对参考坐标系的方位，可以完全由动坐标系依次绕三个不同的转轴的转角来确定。姿态角就属于欧拉角。在将三维矢量代数推广至乘法和除法运算的研究中，爱尔兰数学家、物理学家哈密顿于1843年创建了四元数（(quaternion)和四元数代数。四元数是指由一个实数单位1和三个虚数单位i，j，k组成并具有下列形式实元的数。单位1，i，j，k可以看作四维空间（用符号H来表示)中的一组单位矢量。

在将三维矢量代数推广至乘法和除法运算的研究中，爱尔兰数学家、物理学家哈密顿于1843年创建了四元数（quaternion)和四元数代数。四元数是由实数和虚数的概念推导而来，方向余弦矩阵(Direction Cosine Matrix，DCM）又被称为“坐标转换矩阵”，常用于将矢量的投影从一个坐标系变换到另一坐标系中。方向余弦即与坐标轴夹角的余弦。