前言

最近在研究基于EKF的IMU姿态解算，阅读论文《A Double-Stage Kalman Filter for Orientation Tracking With an Integrated Processor in 9-D IMU》，遇到了四元数的微分方程，公式如下：

其中表示如下：

关于该公式的推导，之前在《惯性导航（第二版）》看过，现在将这部分内容整理下来，防止经常忘记，督促自己养成记笔记的好习惯。

推导过程：

首先定义坐标系：n系为导航坐标系，b系为载体坐标系，则n系到b系的旋转四元数可以表示为：

上式中为旋转轴，为旋转角，对两边求导可得：

根据哥氏定理可得：

由于刚体绕μ轴旋转，与刚体固联的b坐标系的各个轴在旋转的过程中分别位于三个不同的圆锥面上，三个圆锥面的定点即为b系的原点，μ为其共同的对称轴，这块大家可以想象一下，还是挺容易想象的，这样μ到b坐标系三个轴上的投影不变，长度为各自圆锥底面半径，所以有：

又有：

上式中的意思是：R系到b系的角速度在R系上的投影。

所以：

因此

又因为：

上面公式中是纯单位四元数相乘，根据四元数乘法法则容易推出，详细证明可见附录，所以

可得：

上面公式中的是在导航坐标系下的角速度，而IMU中的陀螺仪测量得到的角速度是在载体坐标系的，所以还需要一个转换关系,根据坐标变换的四元数乘法表示法：

上式中为的共轭四元数，所以

带入的公式得：

由于为单位四元数，所以

为陀螺仪的测量值，记

根据四元数的乘法定义，有两种表示形式，第一种如下所示：

即

或者也可以写成如下形式：

即为：

实现过程：

实际上就是四元数微分方程的解法，常用的有欧拉方法、中值法，毕卡算法，龙格库塔法。

常用的是经典4阶龙格库塔法，公式如下所示：

上面公式中的，，，都是微分方程的一阶导数，即为微分方程中的，同时可以看到一阶导数是关于的函数，即，所以在计算，，，时，只需要更新，，，就可以了，是陀螺仪数据更新周期。

参考代码如下：

% 四元数微分方程的4阶龙格库塔法
% q0:4*1
% gyro：陀螺仪数据
% T：更新周期
function [ q ] = Quaternion_RungeKutta4( q0,gyro,T)q0=Norm_Quaternion(q0); %归一化K1= Quaternion_Diff( gyro,q0);q1=Norm_Quaternion(q0+T/2*K1);K2 = Quaternion_Diff(gyro,q1);q2=Norm_Quaternion(q0+T/2*K2);K3 = Quaternion_Diff(gyro,q2);q3=Norm_Quaternion(q0+T*K3);K4 = Quaternion_Diff(gyro,q3);q = q0 + T/6*(K1+2*K2+2*K3+K4);q = Norm_Quaternion(q);
end% 函数功能：四元数微分方程
% 输    出：四元数的一阶导数
% 备    注：连续域
function [ q_diff ] = Quaternion_Diff( gyro,q)A = [       0, -gyro(1)/2, -gyro(2)/2, -gyro(3)/2;gyro(1)/2,          0,  gyro(3)/2, -gyro(2)/2;gyro(2)/2, -gyro(3)/2,          0,  gyro(1)/2;gyro(3)/2,  gyro(2)/2, -gyro(1)/2,         0];q_diff = A*q;
endfunction [ q ] = Norm_Quaternion( q )q = q/norm(q,2);
end

附录：

其中代表实部，代表虚部，'为的转置，当和为纯四元数时，

参考：

1.《惯性导航（第二版）》秦永元

2. Quaternion kinematics for the error-state Kalman filter

3. A Double-Stage Kalman Filter for Orientation Tracking With an Integrated Processor in 9-D IMU

4. https://zh.wikipedia.org/wiki/%E6%AC%A7%E6%8B%89%E6%96%B9%E6%B3%95

6. 常微分方程组作图（一）：龙格-库塔法 - 知乎

四元数微分方程的推导和代码实现相关推荐

INS/GNSS组合导航（七）-SINS的微分方程的推导
(三)中对SINS的机械编排进行了初步可行性的介绍,并未对机械编排进行原理性介绍.那么在详细介绍机械编排之前,需要先对SINS的微分方程进行详细的推导. 无论是机械编排,还是后面误差方程的建立,SIN ...
016四元数微分方程
果然,我还是没有看平台式惯导的初始对准和高度通道,而是直接来到了捷联惯导的部分.之前看过四元数部分的内容,所以现在就当是复习了.回顾一下我之前做的笔记,好像没有四元数微分方程,所以还是在这记录一下吧, ...
MSCKF理论推导与代码解析
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达在SLAM后端中,主要有两种主流方法用于优化:基于滤波的方法和基于非线性的方法.基于滤波的方法主要有M ...
原创 | 机器学习数学推导与代码实现30讲.pdf
机器学习 Author:louwill Machine Learning Lab 机器学习数学推导与代码实现30讲已完成,主要包括监督学习模型.无监督学习模型.集成学习模型和概率模型四个大类29个模型 ...
AES128加密-S盒和逆S盒构造推导及代码实现
AES128加密-S盒和逆S盒构造推导及代码实现 1.1 S盒产生概述文档引用了<密码编码学与网络安全–原理和实践>里边的推导过程,如有不妥,请与我联系修改. 文档<FIPS 19 ...
双线性插值算法推导及代码实现
双线性插值,是一种比较重要的插值方法,尤其在数字图像处理领域.本篇博文分为三个部分:一是双线性插值的算法推导,二是双线性插值的算法实现,三是算法的运行结果. 一双线性插值的算法推导二代码实现(m ...
3D三维向量的单位化、正规化、标准化推导及代码实现
3D三维向量的单位化.正规化.标准化推导及代码实现可以用来法向量等向量的单位化
ROS漫漫长路（一）——Gazebo中机器人圆柱，球，长方体惯性矩阵推导与代码实现
ROS漫漫长路(一)--Gazebo中机器人圆柱,球,长方体惯性矩阵推导与代码实现目录 ROS漫漫长路(一)--Gazebo中机器人圆柱,球,长方体惯性矩阵推导与代码实现前言一.公式推导一.代 ...
四阶龙格库塔法的基本思想_Runge-Kutta法求四元数微分方程
Runge-Kutta法求四元数微分方程 Runge-Kutta法求四元数微分方程文章目录一.背景知识1. 坐标系 2. 四元数四元数的矩阵形式四元数与旋转的关系二.数学模型1. 四元数微分方程 ...

四元数微分方程的推导和代码实现

前言

推导过程：

实现过程：

附录：

参考：

四元数微分方程的推导和代码实现相关推荐

最新文章

热门文章