最近開始着手做碩士畢設課題，需要用到IMU采集姿態信息用以提取手勢，本着親身實踐的目的，我決定自己寫算法實現姿態的融合，由於IMU運動過程中的加速度計輸出疊加了非重力項，然后這就必然繞不開使用原始傳感器的角速度數據來對姿態進行積分運算。
這里值得一提的是，看到網絡上好多博客的做法都是對單個歐拉角直接積分，事實上這種做法是錯誤的，四元數4個參數就可以表示姿態，歐拉角憑什么可以只用3個角度變量表示姿態，是因為歐拉角表示法還有旋轉順序這一條，同一組3個角度，旋轉順序不同，最終的姿態也不同，如果直接對單個歐拉角進行積分，順序就會錯亂，最終的效果無法保證，當然我沒有試驗過，這里只是從理論上說明一下。
正文.
一、旋轉矩陣求導和積分
3維旋轉矩陣表示的是空間物體的姿態，線速度不會改變物體姿態，只有角速度才會。我們先將旋轉矩陣由求極限求導的公式列出來，如式(1):
$$ \dot{R}=\lim\limits_{\triangle{t}\to0}\frac{R(t+\triangle{t})-R(t)}{\triangle{t}} \tag{1} $$
由於Δt前后矩陣仍然是旋轉矩陣，所以必然存在另一個旋轉變換矩陣使得該積分前的旋轉矩陣左乘該變換矩陣可以得到積分后的旋轉矩陣，公式表達如式(2)：
$$ R(t+\triangle{t})=R_{k}(\triangle{\theta})R(t) \tag{2} $$
在上式(2)中變換矩陣用軸角的形式表示，即繞着轉軸K(下標所示)旋轉Δθ角。將(2)代入(1)中，我們得到式(3)：
$$ \dot{R}=(\lim\limits_{\triangle{t}\to0}\frac{R_{k}(\triangle{\theta})-E}{\triangle{t}})R(t) \tag{3} $$
當Δt趨於0，即Δθ趨於0時，可以對軸角參數表示的姿態矩陣(《機器人學》.Craig原書公式 .2.80)進行簡化，得到式(4):
$$ R_{k}(\triangle{\theta})=\left[ \begin{matrix} 1 & -k_z\triangle{\theta} & k_y\triangle{\theta} \\ k_z\triangle{\theta} & 1 & -k_x\triangle{\theta} \\ -k_y\triangle{\theta} & k_x\triangle{\theta} & 1 \end{matrix} \right] \tag{4} $$
再將式(4)代入式(3)，得到式(5)：
$$ \dot{R}=\left[ \begin{matrix} 0 & -k_z\dot{\theta} & k_y\dot{\theta} \\ k_z\dot{\theta} & 0 & -k_x\dot{\theta} \\ -k_y\dot{\theta} & k_x\dot{\theta} & 0 \end{matrix} \right]R(t) \tag{5} $$
上式(5)中的矩陣稱為角速度反對稱矩陣，矩陣的每一項分別表示角速度在X-Y-Z軸上的分量，可由角速度向量叉乘的形式得到，於是最終的由空間角速度和時間積分得到的旋轉矩陣表示的姿態由式(6)得到：
$$ R(t+\triangle{t})=\left[ \begin{matrix} 1 & -\omega_{z}\triangle{t} & \omega_{y}\triangle{t} \\ \omega_{z}\triangle{t} & 1 & -\omega_{x}\triangle{t} \\ -\omega_{y}\triangle{t} & \omega_{x}\triangle{t} & 1 \end{matrix} \right]R(t) \tag{6} $$
二、四元數求導和積分
由公式(6)知道旋轉矩陣的積分形式后，再根據四元數與旋轉矩陣相互轉化的公式(我目前用這種方法只算出了q4，其它三個元素這樣算很麻煩，后面有計算步驟是使用四元數直接微分算得，下面的計算就權當兩種形式的積分之間的相互驗證吧)，我們可以很快的推算出四元數的角速度積分形式，步驟如下：
首先我們列出旋轉矩陣轉化為四元數的公式(7)-(10)：
$$ q_{1}=\frac{r_{32}-r_{23}}{4q_{4}} \tag{7} $$
$$ q_{2}=\frac{r_{13}-r_{31}}{4q_{4}} \tag{8} $$
$$ q_{3}=\frac{r_{21}-r_{12}}{4q_{4}} \tag{9} $$
$$ q_{4}=\frac{1}{2}\sqrt{1+r_{11}+r_{22}+r_{33}} \tag{10} $$
上式中，q4為四元數的實數項，其它為虛數項，接下來我們直接利用式(6)，將其矩陣相乘得到積分后的矩陣表示，即式(11):
$$ R(t+\triangle{t})=\left[ \begin{matrix} 1 & -\omega_{z}\triangle{t} & \omega_{y}\triangle{t} \\ \omega_{z}\triangle{t} & 1 & -\omega_{x}\triangle{t} \\ -\omega_{y}\triangle{t} & \omega_{x}\triangle{t} & 1 \end{matrix} \right]\cdot\left[\begin{matrix} m_{11} & m_{12} & m_{13} \\ m_{21} & m_{22} & m_{23} \\ m_{31} & m_{32} & m_{33} \end{matrix} \right] \tag{11} $$
我以求取新的Q4為例，說明計算的方法，其它三項原理相同，最后我會放出完整的積分公式。下式中，我將以小寫字母m表示積分前的矩陣元素，小寫字母r表示積分后的矩陣元素，大寫字母Q表示新的四元數，小寫q表示積分前的四元數。在式(10)中我們知道，計算q4需要r11,r22,r33,把式(11)乘開即可得到以上三項的表達式分別為：
$$ r_{11} = m_{11}+\triangle{t}(-\omega_{z}m_{21}+\omega_{y}m_{31}) \tag{12} $$
$$ r_{22} = m_{22}+\triangle{t}(\omega_{z}m_{12}-\omega_{x}m_{32}) \tag{13} $$
$$ r_{33} = m_{33}+\triangle{t}(-\omega_{y}m_{13}+\omega_{x}m_{23}) \tag{14} $$
將以上三式代入式(10)可以得到：
$$ Q_{4}=\frac{1}{2}\sqrt{1+m_{11}+m_{22}+m_{33}-\triangle{t}\omega_{z}(m_{21}-m_{12})-\triangle{t}\omega_{y}(m_{13}-m_{31})-\triangle{t}\omega_{x}(m_{32}-m_{23})} \tag{15} $$
進一步變形我們可以得到：
$$ Q_{4}=\frac{1}{2}\sqrt{1+m_{11}+m_{22}+m_{33}}\sqrt{1+\frac{-\triangle{t}\omega_{z}(m_{21}-m_{12})-\triangle{t}\omega_{y}(m_{13}-m_{31})-\triangle{t}\omega_{x}(m_{32}-m_{23})}{1+m_{11}+m_{22}+m_{33}}} \tag{16} $$
將式(7)-(10)代入式(16)中，可得到：
$$ Q_{4} = q_{4}\sqrt{1+\frac{-\omega_{z}\triangle{t}q_{3}-\omega_{y}\triangle{t}q_{2}-\omega_{x}\triangle{t}q_{1}}{q_{4}}} \tag{17} $$
進一步針對$ \sqrt{1+x} $形式的函數進行泰勒展開取前兩項，並應用於式(17)得到近似的解(Δt趨於0時)：
$$ Q_{4} = q_{4}+\frac{1}{2}\triangle{t}(-\omega_{z}q_{3}-\omega_{y}q_{2}-\omega_{x}q_{1}) \tag{17} $$
直接使用四元數微分的計算方法
有四元數的表示形式$ Q=cos(\frac{\theta}{2})+\hat{n}sin(\frac{\theta}{2}) $，對其求導得到式(18):
$$ \frac{dQ}{dt}=-\frac{1}{2}sin(\frac{\theta}{2})\frac{d\theta}{dt}+\frac{d\hat{n}}{dt}sin(\frac{\theta}{2})+\hat{n}\frac{1}{2}cos(\frac{\theta}{2})\frac{d\theta}{dt} \tag{18} $$
上式中$ \hat{n} $表示的為旋轉軸，在積分或者微分的一瞬間，角速度為恆定值，故只有角度變換，$ \frac{d\hat{n}}{dt} $即為0，同時有$ \hat{n}\frac{d\theta}{dt}=\omega^E $，$ \omega^E $表示在世界坐標系中的物體轉動的角速度。在上式中，提取$ \frac{1}{2}\omega^E $，兩外兩項相加即為四元數Q，於是有:
$$ \frac{dQ}{dt}= \frac{1}{2}\omega^E * Q \tag{19}$$
上式中" * "表示4元數乘法。另外設在物體坐標系中的角速度表示為$ \omega^O $，由坐標變換的四元數表示法得到：
$$ \omega^E=Q*\omega^O*Q^* \tag{20}$$
上式中Q*為Q的共軛四元數，Q**Q=1，將上式代入到式(19)得到
$$ \frac{dQ}{dt}= \frac{1}{2}Q*\omega^O \tag{21}$$
將式(20)用四元數乘法展開可得微分的形式：
$$ \frac{dQ}{dt}=\frac{1}{2}\left[ \begin{matrix} 0 & -\omega_x & -\omega_y & -\omega_z \\ \omega_x & 0 & \omega_z & -\omega_y \\ \omega_y & -\omega_z & 0 & \omega_x \\ \omega_z & \omega_y & -\omega_x & 0 \end{matrix} \right]\left[ \begin{matrix} q_4 \\ q_1 \\ q_2 \\ q_3 \end{matrix} \right] \tag{22} $$
然后我們將式(22)代入到微分求極限中，最終可獲得積分形式：
$$ Q=\left[ \begin{matrix} q_4 \\ q_1 \\ q_2 \\ q_3 \end{matrix} \right]+\frac{\triangle{t}}{2}\left[ \begin{matrix} -\omega_xq_1-\omega_yq_2-\omega_zq_3 \\ \omega_xq_4-\omega_yq_3+\omega_zq_2 \\ \omega_xq_3+\omega_yq_4-\omega_zq_1 \\ -\omega_xq_2+\omega_yq_1+\omega_zq_4 \end{matrix} \right] \tag{23} $$
可以看到上式中第一項和我們先前推導的公式相同,但需要注意的是，兩者公式中的角速度並不是同一個參考系下的，所以兩種方法算出的四元數后3項形式是不同的，至於為什么角速度參考坐標系不同卻能得到同一個形式，留給讀者去探索吧！
三、Matlab代碼驗證
https://github.com/ShieldQiQi/Kalman-Filter-Demos