組合導航學習筆記--精確慣性導航

由於老師散養學生，只能自學，最近在看緊組合方面的代碼，看到大佬的代碼中有關於精確計算慣性導航的相關方程，於是學習一下，記錄一下學習心得。學習過之後 才知道 這種所謂的精確計算慣性導航的方式其實就是方向餘弦法求解，之前一直用的是四元數法。

所謂精確計算其實也就是在姿態更新過程中，採用羅德里格斯公式計算載體姿態的變化，然後通過精確計算的計算出載體在i系和e系下姿態的轉變：

這裏的羅德里格斯公式是採用了四階近似的方法，精度較高，羅德里格斯公式如下（直接拷貝書上的公式）：

知道精確的就可以精確的計算出，公式如下：

繼而順利計算比力

至此，可以順利計算出速度和位置。

疑問：之前看秦老師的課本上講到，方向餘弦法對姿態矩陣微分方程求解，可以實現全姿態工作，是包含有9個未知量的線性微分方程組，我咋木有看到9個未知量的線性方程呢？希望有大佬看到這篇寫的賊亂的博客可以指點一下，感恩的心。

致謝：感謝北航劉學長對代碼的講解。