主函式

%捷聯慣導模擬主程式
%東北天座標系，東X北Y天Z

clear
clc

glvs;           % 載入全域性變數

% 子樣數和取樣時間
nn = 2;         % 子樣數，下面也將採用二子樣的圓錐誤差補償演算法
ts = 0.1;       % 單個取樣間隔的取樣時間長度
nts = nn * ts;  % 取樣週期，即在nts時間內進行兩次取樣，每次取樣0.1秒

%姿態、速度和位置初始化
att = [0; 0; 30] *arcdeg;            % 初始化姿態角，分別為俯仰角θ、橫滾角γ、航向角ψ
vn = [0; 0; 0];                      % 初始化速度
pos = [34*arcdeg; 108*arcdeg; 100];  % 初始化位置，緯度34度，經度108度，高度100m
qnb = a2qua(att);                    % 姿態角轉換為四元數 b to n

%模擬靜態IMU資料
eth = earth(pos, vn);                  % 地球導航引數計算
wm = qmulv(qconj(qnb), eth.wnie) *ts;  % 地球自轉角速度由導航系換算到機體系，作為機體角速度，並乘時間得角度增量
vm = qmulv(qconj(qnb), -eth.gn) *ts;   % 比力由導航系換算到機體系，作為機體比力，並乘時間得速度增量
wm = repmat(wm', nn, 1);               % 轉置角速度向量，並複製得到nn行'1'列
vm = repmat(vm', nn, 1);               % 轉置速度向量，並複製得到nn行'1'列

%模擬時長
phi = [0.1; 0.2; 3] *arcmin;   % 失準角誤差
qnb = qaddphi(qnb, phi);       % 新增失準角誤差的由機體系到導航系的四元數
len = fix(3600/ts);            % 模擬時長一小時，得到fix(3600/ts)個子樣

%記錄導航結果 [att, vn, pos, t]
avp = zeros(len, 10);
kk = 1;
t = 0;

% 共有len個子樣，每個取樣時間段nn個子樣，共len/nn個時間段，迴圈len/nn步
for k = 1:nn:len         
    t = t +nts;                             % t表示取樣時間段
    [qnb, vn, pos] = insupdate(qnb, vn, pos, wm, vm, ts);
    vn(3) = 0;                              % 高度不變
    avp(kk,:) = [q2att(qnb); vn; pos; t]';  % 四元數轉換為姿態角
    kk = kk +1;
    if mod(t, 500) < nts
        disp( fix(t) );                     % 顯示進度
    end
end
avp(kk:end, :) = [];                        % 18000行之後設定為空
tt = avp(:, end);                           % tt為avp最後一列，時間

mysubplot(221, tt, avp(:, 1:2) /arcdeg, '\theta, \gamma /\circ');
mysubplot(222, tt, avp(:, 3) /arcdeg, '\psi /\circ');
mysubplot(223, tt, avp(:, 4:6), 'v^n / m/s');
mysubplot(224, tt, deltapos(avp(:, 7:9)), '\Deltap / m');
 

現在回頭看看捷聯慣導模擬，其實也沒那麼難，主線可以這樣： $\text{初始化} \xrightarrow{} \text{模擬靜態IMU資料} \xrightarrow{} \text{更新導航資訊} \xrightarrow{} \text{繪圖}$ 只不過需要細化來看。

(1)初始化

設定子樣數、取樣時間； 姿態、速度、位置初始化不必多說，還要根據姿態求四元數，注意這是從機體系到導航系的四元數表示； 地球導航引數計算，主要是為了得到導航系相對慣性系的角速度，當然這裡是角速度增量，另外得到有害加速度的速度增量。

（2）模擬靜態IMU資料

慣性器件靜態，涉及到的角速度就是隨地球自轉的角速度，比力為重力向量逆向，因此將導航系中的角速度速度通過四元數（注意qconj(qnb)）換算到機體系角速度並乘時間得到角度增量，同樣獲得速度增量； 添加了失準角誤差，並將四元數轉換為考慮失準角誤差的情況。

（3）更新導航資訊

這是該程式中最為核心的部分，其中還包含對等效旋轉向量的圓錐誤差補償、對速度的旋轉與划船誤差的補償； 更新中要注意座標系的關係，例如將補償後的速度增量dvbm先轉換到導航系，在進行速度更新。

（4）繪圖

此處略去十萬字！

從開始學習慣導到現在，一直在圍繞這個程式進行，今天終於大體弄明白了，心情愉悅萬分。這一個半月以來，從對慣導的一無所知到入門水平，就像當初爬泰山時翻過十八盤一樣暢快—狠狠地邁上了一大步（如果把學會慣導的精髓同登上泰山比較，那我這個比喻太不恰當了哈哈）！ 自從高中畢業之後，這也是少有的如此全神貫注地學習，這讓我更加堅信環境是多麼重要，讓我更加堅定了當初自己的選擇！ 對於慣導的學習，還是不能放鬆，感覺距離進行慣導的研究還比較遠，所以下一步打算把講義看一遍，算是查漏補缺，當然還要嘗試自己寫程式，並進行某些演算法的研究。 馬上中秋了，估計還是捨不得離開辦公室一天！現在終於體會到惜時如金的感受！希望十月一給自己放一天假，放鬆一下！ 隨便寫寫感受，反正也沒人知道我是誰哈哈！！