具體程式碼：在原始檔ins-init-rt.cc中的insinirtobs函式中（line：148）
對INS初始化，主要是給出載體相對於導航座標系的初始位置、初始速度和初始姿態資訊，
求初始姿態最及求出載體座標系相對導航座標系的尤拉角即可
此程式中認為pitch和roll這兩個角都為0，用載體在導航座標系下的速度向量求yaw

1.求出合格的RTK定位結果，然後用結果求速度向量(此時的速度向量是在e-frame)

先把RTK計算出的結果存在sols裡面；
for (i=0;i<MAXSOL-1;i++) sols[i]=sols[i+1]; sols[i]=rtk.sol;
儲存滿後，檢查儲存的結果是否符合標準
for (i=0;i<MAXSOL;i++) {
    if (sols[i].stat>popt.insopt.iisu||sols[i].stat==SOLQ_NONE) {
        trace(2,"check solution status fail\n");
        return 0;
    }
}
 

“popt.insopt.iisu”是配置檔案中設定的，作者設定的是2，只有固定解才符合標準，自己可以根據自己資料質量具體設定；

ins-iisu           =2          # initial ins state use rtk options (SOLQ_???)#define SOLQ_NONE    0                  /* solution status: no solution */
#define SOLQ_FIX     1                  /* solution status: fix */
#define SOLQ_FLOAT   2                  /* solution status: float */
#define SOLQ_SBAS    3                  /* solution status: SBAS */
#define SOLQ_DGPS    4                  /* solution status: DGPS/DGNSS */
#define SOLQ_SINGLE  5                  /* solution status: single */
#define SOLQ_PPP     6                  /* solution status: PPP */
#define SOLQ_DR      7                  /* solution status: dead reconing */
#define SOLQ_DOP     8                  /* solution status: doppler measurement aid */
#define SOLQ_INHERIT 9                  /* solution status: ambiguity inherit fix status */
 

如果資料都是固定解，再檢查它們的時間間隔有沒有超限；

for (i=0;i<MAXSOL-1;i++) {
    if (timediff(sols[i+1].time,sols[i].time)>MAXDIFF) {
        return 0;
    }
}

2.根據位置（GNSS求出的載體位置）求出Cne(n-frame-->e-frame的方向餘弦陣)  ，把速度向量轉到n-frame
  然後yaw = arctan（Ve/Vn）/*Ve：東向速度，Vn：北向速度*/

認為roll、pitch為零，即可求出Cnb(n-frame-->b-frame的方向餘弦陣)