在IMU惯性导航误差分析建模中 一共有三个微分方程 分别对应姿态误差微分方程 速度误差微分方程和位置误差微分方程
n为东北天坐标系 b为机体坐标系 这里求出二者之间的转换关系就可以求出IMU的姿态 这里旋转关系用旋转矩阵C表示 对这个旋转矩阵求导: ω n b b \omega^b_{nb} ωnbb为从n系转到b系 并且这个旋转在b系下的表示 这个不容易进行测量 所以将这个角速度进行拆分 最后得到: 上面的方程中有三个变量 这三个变量均含有误差 下面对这三个带有误差的变量进行逐一分析
头上带波浪线的表示这个变量包含了误差 我们要求的就是n系和b系之间的旋转矩阵 但是这个旋转矩阵包含噪声 现在我们做以下认为: 我们认为这个旋转矩阵的误差在n系上 带误差的 旋转矩阵将b系旋转到了n’ 系而不是n系 所以需要一个n系和n’ 系的旋转矩阵 从n系到n’ 系 我们用旋转矢量 ϕ \phi ϕ来表示 旋转矢量的方向为绕着轴旋转轴的方向 大小或者模长为旋转的角度 旋转矢量其实包含三个元素 称为失准角 在后面状态转移方程中要用到 有罗德里格斯公式: 因为旋转矢量趋近于0 所以最后结果为
后面的误差项其实就是IMU内参的误差 展开为: 其中 ε \varepsilon ε为陀螺仪的bias
不是做导弹这种惯性导航的话 这两项误差太小 不作考虑
中间化简过程就省略了 难度不是很大 展示最后化简结果:
带误差的微分方程: f为加速度 上面的公式中四个变量均有误差 下面进行分解: 最后一顿带 得到最后结果:
这个就比较简单了 直接给出结果了:
这里的状态转移方程可以理解为卡尔曼滤波中的状态转移方程 首先我们将上面三个误差微分方程做进一步展开 首先是位姿误差微分方程展开
误差微分方程为 展开: 这里的 ϕ \phi ϕ是我们之前提到过的旋转矢量 它包括三个方向的失准角 ω \omega ω为地球自转角速度 L L L为纬度值 ε \varepsilon ε为b系下的测量值 需要转到n系下
道理跟上面位姿误差微分方程展开差不多 这里不做过多解释 其中 ∇ \nabla ∇为加速度计的bias
过于简单不做解释
状态方程一般形式为: 状态量为: 其中F矩阵为X状态量各个元素之间的转移关系 脑补一下 横着是五个状态的元素 竖着也是五个状态的元素 例如 F 23 F_{23} F23为速度误差的导数和姿态误差的关系 W为器件的噪声: ω g \omega_g ωg为陀螺仪的噪声 ω a \omega_a ωa为加速度计的噪声 这一部分是F中的后半部分 也是可以求导获得的 因为噪声是和bias以同样的方式输入进来的