【IMU】【卡尔曼滤波】惯性导航误差微分方程与状态转移方程

惯性导航误差微分方程

在IMU惯性导航误差分析建模中 一共有三个微分方程 分别对应姿态误差微分方程 速度误差微分方程和位置误差微分方程

姿态误差微分方程

n为东北天坐标系 b为机体坐标系 这里求出二者之间的转换关系就可以求出IMU的姿态 这里旋转关系用旋转矩阵C表示 对这个旋转矩阵求导：

${\omega }_{nb}^b$为从n系转到b系 并且这个旋转在b系下的表示 这个不容易进行测量 所以将这个角速度进行拆分 最后得到：

上面的方程中有三个变量 这三个变量均含有误差 下面对这三个带有误差的变量进行逐一分析

旋转向量

头上带波浪线的表示这个变量包含了误差
我们要求的就是n系和b系之间的旋转矩阵 但是这个旋转矩阵包含噪声 现在我们做以下认为：

我们认为这个旋转矩阵的误差在n系上 带误差的 旋转矩阵将b系旋转到了n’ 系而不是n系 所以需要一个n系和n’ 系的旋转矩阵
从n系到n’ 系 我们用旋转矢量$\varphi$来表示 旋转矢量的方向为绕着轴旋转轴的方向 大小或者模长为旋转的角度
旋转矢量其实包含三个元素 称为失准角 在后面状态转移方程中要用到
有罗德里格斯公式：

因为旋转矢量趋近于0 所以最后结果为

角速度 i 到 b

后面的误差项其实就是IMU内参的误差 展开为：

其中$\epsilon$为陀螺仪的bias

角速度 i到 n

不是做导弹这种惯性导航的话 这两项误差太小 不作考虑

化简结果

中间化简过程就省略了 难度不是很大 展示最后化简结果：

速度误差微分方程

带误差的微分方程：

f为加速度
上面的公式中四个变量均有误差 下面进行分解：

最后一顿带 得到最后结果：

位置误差微分方程

这个就比较简单了 直接给出结果了：

状态转移方程

这里的状态转移方程可以理解为卡尔曼滤波中的状态转移方程
首先我们将上面三个误差微分方程做进一步展开
首先是位姿误差微分方程展开

位姿误差微分方程展开

误差微分方程为

展开：

这里的$\varphi$是我们之前提到过的旋转矢量 它包括三个方向的失准角
$\omega$为地球自转角速度 $L$为纬度值
$\epsilon$为b系下的测量值 需要转到n系下

速度误差微分方程展开

道理跟上面位姿误差微分方程展开差不多 这里不做过多解释
其中$\nabla$为加速度计的bias

位置误差微分方程展开

过于简单不做解释

整合进状态转移方程中

状态方程一般形式为：

状态量为：

其中F矩阵为X状态量各个元素之间的转移关系

脑补一下 横着是五个状态的元素 竖着也是五个状态的元素
例如$F_{23}$为速度误差的导数和姿态误差的关系


W为器件的噪声：

${\omega }_g$为陀螺仪的噪声 ${\omega }_a$为加速度计的噪声

这一部分是F中的后半部分 也是可以求导获得的 因为噪声是和bias以同样的方式输入进来的