【IMU】【卡尔曼滤波】惯性导航误差微分方程与状态转移方程
1|0惯性导航误差微分方程
在IMU惯性导航误差分析建模中 一共有三个微分方程 分别对应姿态误差微分方程 速度误差微分方程和位置误差微分方程
1|1姿态误差微分方程
n为东北天坐标系 b为机体坐标系 这里求出二者之间的转换关系就可以求出IMU的姿态 这里旋转关系用旋转矩阵C表示 对这个旋转矩阵求导:
为从n系转到b系 并且这个旋转在b系下的表示 这个不容易进行测量 所以将这个角速度进行拆分 最后得到:
上面的方程中有三个变量 这三个变量均含有误差 下面对这三个带有误差的变量进行逐一分析
旋转向量
头上带波浪线的表示这个变量包含了误差
我们要求的就是n系和b系之间的旋转矩阵 但是这个旋转矩阵包含噪声 现在我们做以下认为:
我们认为这个旋转矩阵的误差在n系上 带误差的 旋转矩阵将b系旋转到了n' 系而不是n系 所以需要一个n系和n’ 系的旋转矩阵
从n系到n' 系 我们用旋转矢量来表示 旋转矢量的方向为绕着轴旋转轴的方向 大小或者模长为旋转的角度
旋转矢量其实包含三个元素 称为失准角 在后面状态转移方程中要用到
有罗德里格斯公式:
因为旋转矢量趋近于0 所以最后结果为
角速度 i 到 b
后面的误差项其实就是IMU内参的误差 展开为:
其中为陀螺仪的bias
角速度 i到 n
化简结果
1|2速度误差微分方程
带误差的微分方程:
f为加速度
上面的公式中四个变量均有误差 下面进行分解:
最后一顿带 得到最后结果:
1|3位置误差微分方程
2|0状态转移方程
这里的状态转移方程可以理解为卡尔曼滤波中的状态转移方程
首先我们将上面三个误差微分方程做进一步展开
首先是位姿误差微分方程展开
2|1位姿误差微分方程展开
误差微分方程为
展开:
这里的是我们之前提到过的旋转矢量 它包括三个方向的失准角
为地球自转角速度 为纬度值
为b系下的测量值 需要转到n系下
2|2速度误差微分方程展开
道理跟上面位姿误差微分方程展开差不多 这里不做过多解释
其中为加速度计的bias
2|3位置误差微分方程展开
2|4整合进状态转移方程中
状态方程一般形式为:
状态量为:
其中F矩阵为X状态量各个元素之间的转移关系
脑补一下 横着是五个状态的元素 竖着也是五个状态的元素
例如为速度误差的导数和姿态误差的关系
W为器件的噪声:
为陀螺仪的噪声 为加速度计的噪声
这一部分是F中的后半部分 也是可以求导获得的 因为噪声是和bias以同样的方式输入进来的
__EOF__

本文链接:https://www.cnblogs.com/linglingdog/p/16056103.html
关于博主:评论和私信会在第一时间回复。或者直接私信我。
版权声明:本博客所有文章除特别声明外,均采用 BY-NC-SA 许可协议。转载请注明出处!
声援博主:如果您觉得文章对您有帮助,可以点击文章右下角【推荐】一下。您的鼓励是博主的最大动力!
【推荐】国内首个AI IDE,深度理解中文开发场景,立即下载体验Trae
【推荐】编程新体验,更懂你的AI,立即体验豆包MarsCode编程助手
【推荐】抖音旗下AI助手豆包,你的智能百科全书,全免费不限次数
【推荐】轻量又高性能的 SSH 工具 IShell:AI 加持,快人一步
· 物流快递公司核心技术能力-地址解析分单基础技术分享
· 单线程的Redis速度为什么快?
· 展开说说关于C#中ORM框架的用法!
· Pantheons:用 TypeScript 打造主流大模型对话的一站式集成库
· SQL Server 2025 AI相关能力初探