四元数微分方程的推导和代码实现
前言
最近在研究基于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;
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. https://zhuanlan.zhihu.com/p/47881355