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

惯性导航误差微分方程

在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 所以最后结果为
在这里插入图片描述

角速度 i 到 b

在这里插入图片描述
后面的误差项其实就是IMU内参的误差 展开为:
【IMU】【卡尔曼滤波】惯性导航误差微分方程与状态转移方程_第1张图片
其中 ε \varepsilon ε为陀螺仪的bias

角速度 i到 n

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

化简结果

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

速度误差微分方程

带误差的微分方程:
在这里插入图片描述
f为加速度
上面的公式中四个变量均有误差 下面进行分解:
【IMU】【卡尔曼滤波】惯性导航误差微分方程与状态转移方程_第2张图片
最后一顿带 得到最后结果:
在这里插入图片描述

位置误差微分方程

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

状态转移方程

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

位姿误差微分方程展开

误差微分方程为
在这里插入图片描述
展开:
【IMU】【卡尔曼滤波】惯性导航误差微分方程与状态转移方程_第3张图片
这里的 ϕ \phi ϕ是我们之前提到过的旋转矢量 它包括三个方向的失准角
ω \omega ω为地球自转角速度 L L L为纬度值
ε \varepsilon ε为b系下的测量值 需要转到n系下

速度误差微分方程展开

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

位置误差微分方程展开

【IMU】【卡尔曼滤波】惯性导航误差微分方程与状态转移方程_第4张图片
过于简单不做解释

整合进状态转移方程中

状态方程一般形式为:
在这里插入图片描述
状态量为:
在这里插入图片描述
其中F矩阵为X状态量各个元素之间的转移关系
【IMU】【卡尔曼滤波】惯性导航误差微分方程与状态转移方程_第5张图片
脑补一下 横着是五个状态的元素 竖着也是五个状态的元素
例如 F 23 F_{23} F23为速度误差的导数和姿态误差的关系
【IMU】【卡尔曼滤波】惯性导航误差微分方程与状态转移方程_第6张图片
【IMU】【卡尔曼滤波】惯性导航误差微分方程与状态转移方程_第7张图片
W为器件的噪声:
在这里插入图片描述
ω g \omega_g ωg为陀螺仪的噪声 ω a \omega_a ωa为加速度计的噪声
【IMU】【卡尔曼滤波】惯性导航误差微分方程与状态转移方程_第8张图片
这一部分是F中的后半部分 也是可以求导获得的 因为噪声是和bias以同样的方式输入进来的

你可能感兴趣的:(矩阵,线性代数,算法)