惯性导航误差微分方程

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

姿态误差微分方程

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

ωnbb\omega^b_{nb}ωnbb为从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ω为地球自转角速度 LLL为纬度值
ε\varepsilonε为b系下的测量值需要转到n系下

速度误差微分方程展开

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

位置误差微分方程展开

过于简单不做解释

整合进状态转移方程中

状态方程一般形式为：

状态量为：

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

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

W为器件的噪声：

ωg\omega_gωg为陀螺仪的噪声 ωa\omega_aωa为加速度计的噪声

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

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

【滤波跟踪】基于matlab无迹卡尔曼滤波惯性导航+DVL组合导航【含Matlab源码 2019期】
⛄一.自主导航技术简介 1 基于SINS/声学的AUV自主导航 1.1 基于SINS/DVL的AUV自主导航技术 DVL是基于声呐多普勒效应的测速设备, 能提供较高精度的载体速度信息, 且其误差不随时 ...
运筹学状态转移方程例子_强化学习第4期：H-J-B方程
在上一篇文章中,我们介绍了一种最简单的MDP--s与a都是有限的MDP的求解方法.其中,我们用到了动态规划的思想,并且推出了"策略迭代"."值迭代"这样的方法. ...
c++ 多重背包状态转移方程_背包问题之零钱兑换
读完本文,你可以去力扣拿下如下题目: 518.零钱兑换II ----------- 零钱兑换 2 是另一种典型背包问题的变体,我们前文已经讲了经典动态规划:0-1 背包问题和背包问题变体:相等子 ...
c++ 多重背包状态转移方程_动态规划入门——详解经典问题零一背包
本文始发于个人公众号:TechFlow,原创不易,求个关注今天是周三算法与数据结构专题的第12篇文章,动态规划之零一背包问题. 在之前的文章当中,我们一起探讨了二分.贪心.排序和搜索算法,今天我们来 ...
运筹学状态转移方程例子_如何确定动态规划的转移方程
先观察最后一个状态比如 LintCode 114 ,题意是从一个二维数组的左上角走到右下角共有多少种不同的路径.且每次只能向下或者向右走一步,最后一个状态就是右下角 2. 观察到达最后一个状态的前面 ...
运筹学状态转移方程例子_动态规划 Dynamic Programming
从运筹学和算法的角度综合介绍动态规划规划论 Mathematical Programming / Mathematical Optimization In mathematics, computer ...
【C++】动态规划之状态转移方程（单串）
目录一.单串最长递增子序列(依赖O(n)个子问题) 300. 最长递增子序列 673. 最长递增子序列的个数最大子数组和(依赖O(1)个子问题,可以用滚动数组优化空间复杂度) 53.最大子数组和 ...
算法设计 - 01背包问题的状态转移方程优化，以及完全背包问题
01背包问题的一维状态转移方程的推导前提摘要前面这篇博客中: 算法设计 - 01背包问题_伏城之外的博客-CSDN博客我们已经推导出了01背包问题的二维数组dp状态转移方程公式: 假设有N种不同 ...
100道动态规划——34 UVA 10559 Blocks 状态的定义状态转移方程
好吧,我想到的是枚举决策... 居然是个类似于区间DP的感觉.恩..确实是呀,只不过是需要在DP的时候附加上额外的条件定义状态dp[i][j][k]表示区间i~j且右边附加了k个与j同色的方块的最大 ...

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