1 IMU真实状态运动模型

状态向量:

\(x_{I}=\left[{{_{G}^{I}{q(t)}}^{T},{b_{g}(t)}^{T},{^{G}v_{I}(t)}^{T},{b_{a}(t)}^{T},{^{G}p_{I}(t)}^{T}},{{_{C}^{I}q(t)}^{T}},{{^{I}p(t)_{C}}^{T}}\right]^{T}\)

四元数\({_{G}^{I}{q(t)}}\)代表惯性系到IMU坐标系的旋转,\({^{G}v_{I}(t)}\)和\({^{G}p_{I}(t)}\)代表IMU坐标系在惯性系中的速度和位置,\({b_{g}(t)}\)和\({b_{a}(t)}\)表示在IMU坐标系中测量值角速度与线加速度的biases,\({_{C}^{I}q(t)}\)和\({^{I}p(t)_{C}}\)表示相机坐标系和IMU坐标系的相对位置,其中相机坐标系取左相机坐标系。

数量关系:

\(_{G}^{I}{\dot q(t)}=\frac{1}{2}\Omega(w(t))_{G}^{I}q(t)\)

\(\dot b_{g}(t)=n_{wg}(t)\)

\(^{G}\dot v_{I}(t)={^{G}a_{I}(t)}\)

\(\dot b_{a}(t)=n_{wa}(t)\)

\(^{G}\dot p_{I}(t)={^{G}v_{I}(t)}\)

\({_{C}^{I}{\dot q(t)}}=0_{4\times1}\)

\({^{I}{\dot p}(t)_{C}}=0_{3\times1}\)

以上\(w(t)=[w_{x}(t),w_{y}(t),w_{z}(t)]^{T}\)是IMU角速度在IMU系中的坐标。

IMU的观测值为:

\(w_{m}=w+C(_{G}^{I}q)w_{G}+b_{g}+n_{g}\)

\(a_{m}=C(_{G}^{I}q)\left(^{G}a_{I}-{^{G}g}+2[w_{G}\times]{^{G}v_{I}}+{[w_{G}\times]}^{2}{(^{G}p_{I})}\right)+b_{a}+n_{a}\)

其中\(w_{G}\)为地球的自转速度在G系的坐标,在某些VIO实现中,会将地球自转的影响忽略不计,比如S-MSCKF,以后的推导中也会不计地球自转影响。

将地球自转忽略:

\(w_{m}=w+b_{g}+n_{g}\)

\(a_{m}=C(_{G}^{I}q)\left(^{G}a_{I}-{^{G}g}\right)+b_{a}+n_{a}\)

2 IMU估计状态运动模型

状态向量:

\(\hat x_{I} =\left[{{_{G}^{I}{\hat q}}^{T},{\hat b_{g}}^{T},{^{G}\hat v_{I}}^{T},{\hat b_{a}}^{T},{^{G}\hat p_{I}}^{T}},{_{C}^{I}\hat q}^{T},{^{I}{\hat p}_{C}}^{T}\right]^{T}\)

数量关系:

\(_{G}^{I}{\dot{\hat q}}=\frac{1}{2}\Omega(\hat w)_{G}^{I}\hat q\)

\(\dot {\hat b}_{g}(t)=0_{3\times1}\)

\(^{G}\dot {\hat v}_{I}=C(_{G}^{I}\hat q)^{T}{\hat a}+{^{G}g}\)

\(\dot {\hat b}_{a}=0_{3\times1}\)

\(^{G}\dot {\hat p}_{I}={^{G}{\hat v}_{I}}\)

\({_{C}^{I}\hat q}=0_{4\times1}\)

\({^{I}{\hat p}_{C}}^{T}=0_{3\times1}\)

其中:

\(\hat w = w_{m}-{\hat b}_{g}\)

\(\hat a= a_{m}-{\hat b}_{a}\)

\(\Omega(\hat w)= \left(\begin{matrix} -[\hat w_{\times}] & {\hat w} \\ -{\hat w}^{T} & 0 \end{matrix}\right)\)

3 IMU误差状态运动模型

定义IMU误差状态:

将上述真实状态与估计状态做“差”:

\(\tilde x = x-{\hat x}\)

其中四元数做差和普通的减法不一样,这里引入了误差四元数\(\delta q\)来表示旋转误差:

\(q={\delta q} \bigotimes {\hat q}\)

\({\delta q}\simeq [\frac{1}{2}{\delta\theta}^{T},1]^{T}\)

所以可以用三维向量\(\delta\theta\)来表示旋转误差,从而定义IMU误差状态向量为:

\(\tilde x_{I} =\left[{{_{G}^{I}{\tilde\theta}^{T}},{\tilde b_{g}}^{T},{^{G}\tilde v_{I}}^{T},{\tilde b_{a}}^{T},{^{G}\tilde p_{I}}^{T}},{_{C}^{I}{\tilde\theta}^{T}},{^{I}{\tilde p}_{C}}^{T}\right]^{T}\)

连续误差状态运动方程:

\({\dot {\tilde x}}_{I}=F{\tilde x_{I}}+G{n}_{I}\)

其中\(n_{I}^{T}=\left({n_{g}^{T}},{n_{wg}^{T}},{n_{a}^{T}},{n_{wa}^{T}}\right)^{T}\)。向量\(n_{g}\)和\(n_{a}\)代表陀螺仪与加速度计的测量噪声(高斯),\(n_{wg}\)和\(n_{wa}\)是陀螺仪与加速度计biases的随机游走速率。

4 推导F与G

4.1 求\(\delta\theta\)

对\(_{G}^{I}q=\delta q \bigotimes{_{I}^{G}\hat q}\)左右两边同时求导得到:

\(_{G}^{I}\dot q=\dot {\delta q} \bigotimes {_{G}^{I}\hat q} + \delta q \bigotimes {_{G}^{I}\dot {\hat q}}\)

\(\frac{1}{2}\left[\begin{matrix} w \\ 0 \end{matrix}\right] \bigotimes {_{G}^{I}q}= \dot {\delta q} \bigotimes {_{G}^{I}{\hat q}}+ \delta q \bigotimes \frac{1}{2}\left[\begin{matrix} {\hat w} \\ 0 \end{matrix}\right] \bigotimes {_{G}^{I}{\hat q}}\)

两边同时乘以\({_{G}^{I}{\hat q}}^{-1}\)得到:

\(\frac{1}{2}\left[\begin{matrix} w \\ 0 \end{matrix}\right] \bigotimes {\delta q}= \dot {\delta q}+ \frac{1}{2}\delta q \bigotimes \left[\begin{matrix} {\hat w} \\ 0 \end{matrix}\right]\)

整理得:

\(\left[\begin{matrix} {\dot {\delta\theta}} \\ {\dot {2}} \end{matrix}\right]= \left[\begin{matrix} w \\ 0 \end{matrix}\right] \bigotimes \delta q- \delta q \bigotimes\left[\begin{matrix} {\hat w} \\ 0 \end{matrix}\right]\)

由\(w_{m}=w+b_{g}+n_{g}\)和\(\hat w =w_{m}-{\hat b}_{g}\)可以求出\(w\):

\(w=\hat w + {\hat b}_{g}-b_{g}-n_{g}=\hat w - {\tilde b}_{g}-n_{g}\)

带入\(w\)得到:

\(\left[\begin{matrix} {\dot {\delta\theta}} \\ {0} \end{matrix}\right]= \left[\begin{matrix} {\hat w - {\tilde b}_{g}-n_{g}} \\ 0 \end{matrix}\right] \bigotimes \delta q- \delta q \bigotimes\left[\begin{matrix} {\hat w} \\ 0 \end{matrix}\right]\)

\(\left[\begin{matrix} {\dot {\delta\theta}} \\ {0} \end{matrix}\right]= \left[\begin{matrix} {\hat w} \\ 0 \end{matrix}\right] \bigotimes \delta q- \delta q \bigotimes\left[\begin{matrix} {\hat w} \\ 0 \end{matrix}\right]- \left[\begin{matrix} {{\tilde b}_{g}+n_{g}} \\ 0 \end{matrix}\right] \bigotimes \delta q\)

利用四元数乘法的性质得到:

\(\left[\begin{matrix} {\dot {\delta\theta}} \\ {0} \end{matrix}\right]= \left[\begin{matrix} -[{\hat w}_{\times}] & {\hat w} \\ -{\hat w}^{T} & 0 \end{matrix}\right] \delta q- \left[\begin{matrix} [{\hat w}_{\times}] & {\hat w} \\ -{\hat w}^{T} & 0 \end{matrix}\right]\delta q- \left[\begin{matrix} -[{(\tilde b_{g}+n_{g})}_{\times}] & {(\tilde b_{g}+n_{g})} \\ -{(\tilde b_{g}+n_{g})}^{T} & 0 \end{matrix}\right] \left[\begin{matrix} {\frac{1}{2}\delta\theta} \\ {1} \end{matrix}\right]\)

忽略掉极小量相乘的项得:

\(\left[\begin{matrix} {\dot {\delta\theta}} \\ {0} \end{matrix}\right]= \left[\begin{matrix} -2[{\hat w}_{\times}] & 0 \\ 0 & 0 \end{matrix}\right] \left[\begin{matrix} {\frac{1}{2}\delta\theta} \\ {1} \end{matrix}\right]- \left[\begin{matrix} {\tilde b_{g}+n_{g}} \\ {0} \end{matrix}\right]\)

\({\dot {\delta\theta}}=-[{\hat w}_{\times}]\delta\theta -\tilde b_{g} -n_{g}\)

4.2 求\(\dot{\tilde b}_{g}\)

\(\dot{\tilde b}_{g}=\dot b_{g}-\dot{\hat b}_{g}=n_{wg}\)

4.3 求\(^{G}\dot {\tilde v}_{I}\)

\(^{G}\dot {\tilde v}_{I}={^{G}\dot v_{I}-^{G}\dot {\hat v}_{I}}\)

\(^{G}\dot v_{I}={^{G}a_{I}}={C(_{G}^{I}q)}^{T}\left(a_{m}-b_{a}-n_{a}\right)+{^{G}g}\)

\(^{G}\dot {\hat v}_{I}=C(_{G}^{I}\hat q)^{T}{\hat a}+{^{G}g}\)

\(^{G}\dot {\tilde v}_{I}={C(_{G}^{I}q)}^{T}\left(a_{m}-b_{a}-n_{a}\right)+{^{G}g}-C(_{G}^{I}\hat q)^{T}{\hat a}-{^{G}g}\)

\(={C(_{G}^{I}{\hat q})}^{T}\left(I+[\delta\theta_{\times}]\right)\left(a_{m}-b_{a}-n_{a}\right)-C(_{G}^{I}\hat q)^{T}{\hat a}\)

由\(\hat a=a_{m}-\hat b_{a}\)得:

\(={C(_{G}^{I}{\hat q})}^{T}\left(I+[\delta\theta_{\times}]\right)\left(\hat a-\tilde b_{a}-n_{a}\right)-C(_{G}^{I}\hat q)^{T}{\hat a}\)

省略掉高次项\([\delta\theta_{\times}]\left(-\tilde b_{a}-n_{a}\right)\)得到:

\(={C(_{G}^{I}{\hat q})}^{T}\left([\delta\theta_{\times}]\hat a -\tilde b_{a}-n_{a}\right)\)

\(=-{C(_{G}^{I}{\hat q})}^{T}[\hat a_{\times}]\delta\theta-{C(_{G}^{I}{\hat q})}^{T}\tilde b_{a}-{C(_{G}^{I}{\hat q})}^{T}n_{a}\)

4.4 求\(\dot {\tilde b}_{a}\)

\(\dot {\tilde b}_{a}=\dot b_{a}-\dot {\hat b}_{a}=n_{wa}\)

4.5 求\(^{G}\dot {\tilde p}_{I}\)

\(^{G}\dot {\tilde p}_{I}={^{G}p_{I}}-{^{G}\dot {\hat p}_{I}}\)

\(={^{G}v_{I}}-{^{G}{\hat v}_{I}}={^{G}{\tilde v}_{I}}\)

4.6求\(_{C}^{I}\tilde \theta\)和\(^{I} \tilde p_{C}\)

\(_{C}^{I}\tilde \theta\)和\(^{I} \tilde p_{C}\)是相机与IMU的相对位置,所以有:

\(_{C}^{I}\tilde \theta=0_{3\times 1}\)

\(^{I} \tilde p_{C}=0_{3\times 1}\)

4.7 写成矩阵形式

\[\dot {\tilde x}_{I}=\left[\begin{matrix}
-[\hat w_{\times}] & -I_{3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3}\\
0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \\
-C\left({_{G}^{I}\hat q}\right)^{T}[\hat a_{\times}] & 0_{3 \times 3} & 0_{3 \times 3} & -C\left(^{I}_{G}\hat q\right)^{T} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \\
0_{3 \times 3} & 0_{3 \times 3} & I_{3\times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \\
0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \\
0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3}
\end{matrix}\right]{\tilde x_{I}}+
\left[\begin{matrix}
-I_{3\times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \\
0_{3 \times 3} & I_{3\times 3} & 0_{3 \times 3} & 0_{3 \times 3} \\
0_{3 \times 3} & 0_{3 \times 3} & -C\left(^{I}_{G}\hat q\right)^{T} & 0_{3 \times 3} \\
0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \\
0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & I_{3\times 3} \\
0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \\
0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3}
\end{matrix}\right]{n_{I}}\]

参考资料

(1) A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation

(2) Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight

(3) Indirect Kalman Filter for 3D Attitude Estimation

一步一步学习S-MSCKF(一)连续时间IMU误差状态运动模型的更多相关文章

  1. 12.Linux软件安装 (一步一步学习大数据系列之 Linux)

    1.如何上传安装包到服务器 有三种方式: 1.1使用图形化工具,如: filezilla 如何使用FileZilla上传和下载文件 1.2使用 sftp 工具: 在 windows下使用CRT 软件 ...

  2. (转) 一步一步学习ASP.NET 5 (四)- ASP.NET MVC 6四大特性

    转发:微软MVP 卢建晖 的文章,希望对大家有帮助.原文:http://blog.csdn.net/kinfey/article/details/44459625 编者语 : 昨晚写好的文章居然csd ...

  3. (转) 一步一步学习ASP.NET 5 (二)- 通过命令行和sublime创建项目

    转发:微软MVP 卢建晖 的文章,希望对大家有帮助. 注:昨天转发之后很多朋友指出了vNext的命名问题,原文作者已经做出了修改,后面的标题都适用 asp.net 5这个名称. 编者语 : 昨天发了第 ...

  4. 一步一步学习SignalR进行实时通信_1_简单介绍

    一步一步学习SignalR进行实时通信\_1_简单介绍 SignalR 一步一步学习SignalR进行实时通信_1_简单介绍 前言 SignalR介绍 支持的平台 相关说明 OWIN 结束语 参考文献 ...

  5. 一步一步学习SignalR进行实时通信_8_案例2

    原文:一步一步学习SignalR进行实时通信_8_案例2 一步一步学习SignalR进行实时通信\_8_案例2 SignalR 一步一步学习SignalR进行实时通信_8_案例2 前言 配置Hub 建 ...

  6. 一步一步学习SignalR进行实时通信_9_托管在非Web应用程序

    原文:一步一步学习SignalR进行实时通信_9_托管在非Web应用程序 一步一步学习SignalR进行实时通信\_9_托管在非Web应用程序 一步一步学习SignalR进行实时通信_9_托管在非We ...

  7. 一步一步学习SignalR进行实时通信_7_非代理

    原文:一步一步学习SignalR进行实时通信_7_非代理 一步一步学习SignalR进行实时通信\_7_非代理 SignalR 一步一步学习SignalR进行实时通信_7_非代理 前言 代理与非代理 ...

  8. 一步一步学习SignalR进行实时通信_5_Hub

    原文:一步一步学习SignalR进行实时通信_5_Hub 一步一步学习SignalR进行实时通信\_5_Hub SignalR 一步一步学习SignalR进行实时通信_5_Hub 前言 Hub命名规则 ...

  9. 一步一步学习SignalR进行实时通信_6_案例

    原文:一步一步学习SignalR进行实时通信_6_案例 一步一步学习SignalR进行实时通信\_6_案例1 一步一步学习SignalR进行实时通信_6_案例1 前言 类的定义 各块功能 后台 上线 ...

随机推荐

  1. python列表推导式(扫盲)

    1) 简单了解: 所谓的列表推导式,就是指的轻量级循环创建列表. 格式: 列表推导式的常见形式: my_list = [ item for item in iterable] my_list: 列表名 ...

  2. NoneBot+酷Q,打造QQ机器人

    NoneBot 是一个基于 酷Q 的 Python 异步 QQ 机器人框架,它会对 QQ 机器人收到的消息进行解析和处理,并以插件化的形式,分发给消息所对应的命令处理器和自然语言处理器,来完成具体的功 ...

  3. 后端开发中,可以在Cache-Control设置的常用指令

    max-age 该指令指定从当前请求开始,允许获取的响应被重用的最长时间(单位为秒.例如:Cache-Control:max-age=60表示响应可以再缓存和重用 60 秒.需要注意的是,在max-a ...

  4. myecplise上将工程部署到应用下时,经常出现 An internal error occurred during: "Add Deployment". java.lang.NullPointEx

    myecplise上将工程部署到应用下时,经常出现 An internal error occurred during: "Add Deployment". java.lang.N ...

  5. VirtualBox上使用kubeadm安装Kubernetes集群

    之前一直使用minikube练习,为了更贴近生产环境,使用VirtualBox搭建Kubernetes集群. 为了不是文章凌乱,把在搭建过程中遇到的问题及解决方法记在了另一篇文章:安装Kubernet ...

  6. 达拉草201771010105《面向对象程序设计(java)》第一周学习总结

    达拉草201771010105<面向对象程序设计(java)>第一周学习总结 第一部分:课程准备部分 填写课程学习 平台注册账号, 平台名称 注册账号 博客园:www.cnblogs.co ...

  7. sql的练习题

    表名和字段 –1.学生表 Student(s_id,s_name,s_birth,s_sex) --学生编号,学生姓名, 出生年月,学生性别 –2.课程表 Course(c_id,c_name,t_i ...

  8. CSS 实现元素较宽不能被完全展示时将其隐藏

    首发于本人的博客 varnull.cn 遇到一个需求,需要实现的样式是固定宽度的容器里一排显示若干个标签,数量不定,每个标签的长度也不定.当到了某个标签不能被完全展示下时则不显示.大致效果如下,标签只 ...

  9. Bootstrap4 正式发布

    历经三年开发,前端框架Bootstrap 4正式发布了.然而今天的Web世界已经和当初Mark Otto发布Bootstrap时的情况大为不同,一些开发者由此质疑它的更新是否还有意义 1.V4版本的主 ...

  10. Windows下利用virtualenvwrapper指定python版本创建虚拟环境

    默认已安装virtualenvwrapper 一.添加环境变量(可选) 在系统环境变量中添加 WORKON_HOME ,用来指定新建的虚拟环境的存储位置,如过未添加,默认位置为 %USERPROFIL ...