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

惯性导航误差微分方程

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

姿态误差微分方程

n为东北天坐标系 b为机体坐标系这里求出二者之间的转换关系就可以求出IMU的姿态这里旋转关系用旋转矩阵C表示对这个旋转矩阵求导：
在这里插入图片描述
\(\omega^b_{nb}\)为从n系转到b系并且这个旋转在b系下的表示这个不容易进行测量所以将这个角速度进行拆分最后得到：

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

旋转向量

头上带波浪线的表示这个变量包含了误差
我们要求的就是n系和b系之间的旋转矩阵但是这个旋转矩阵包含噪声现在我们做以下认为：
在这里插入图片描述
我们认为这个旋转矩阵的误差在n系上带误差的旋转矩阵将b系旋转到了n' 系而不是n系所以需要一个n系和n’ 系的旋转矩阵
从n系到n' 系我们用旋转矢量\(\phi\)来表示旋转矢量的方向为绕着轴旋转轴的方向大小或者模长为旋转的角度
旋转矢量其实包含三个元素称为失准角在后面状态转移方程中要用到
有罗德里格斯公式：
在这里插入图片描述
因为旋转矢量趋近于0 所以最后结果为

角速度 i 到 b

在这里插入图片描述
后面的误差项其实就是IMU内参的误差展开为：

其中\(\varepsilon\)为陀螺仪的bias

角速度 i到 n

在这里插入图片描述
不是做导弹这种惯性导航的话这两项误差太小不作考虑

化简结果

中间化简过程就省略了难度不是很大展示最后化简结果：
在这里插入图片描述

速度误差微分方程

带误差的微分方程：
在这里插入图片描述
f为加速度
上面的公式中四个变量均有误差下面进行分解：

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

位置误差微分方程

这个就比较简单了直接给出结果了：
在这里插入图片描述

状态转移方程

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

位姿误差微分方程展开

误差微分方程为
在这里插入图片描述
展开：

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

速度误差微分方程展开

在这里插入图片描述
道理跟上面位姿误差微分方程展开差不多这里不做过多解释
其中\(\nabla\)为加速度计的bias

位置误差微分方程展开

在这里插入图片描述
过于简单不做解释

整合进状态转移方程中

状态方程一般形式为：
在这里插入图片描述
状态量为：

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

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

W为器件的噪声：

\(\omega_g\)为陀螺仪的噪声 \(\omega_a\)为加速度计的噪声
在这里插入图片描述
这一部分是F中的后半部分也是可以求导获得的因为噪声是和bias以同样的方式输入进来的