%%采用WGS-84坐標系
%%長半軸a=6378137m????短半軸b=6356752.3142m???扁率f=1/298.257223563???地球自轉角速度w=7.292115×10^-5?rad/s
%%第一偏心率平方=0.00669437999013??地球引力常數GM=3.986004418×10^14?m^3?/s^2?
%%赤道正常重力=9.7803267714m/s^2???兩級正常重力=9.832185127m/s^2?
%%初始狀態重力加速度為9.788297073
%%初始時間：91620.0?
%%GPS周秒、Gx、Gy、Gz、Ax、Ay、Az?（G代表陀螺，A代表加速度計）
%%初始位置（緯經高）：23.1373950708?[deg]??113.3713651222?[deg]??2.175?[m]
%%初始速度：0.0??0.0??0.0?[m/s]
%%初始姿態（degree）：
%%rollpitchheading分別為0.0107951084511778	-2.14251290749072?-75.7498049314083?
%%由初始姿態角可得，q（41）=[0.789216199658865;-0.0114037805155068;-0.0148154631808315;-0.613830795933790]
%%計算方法：a=0.0107951084511778*pi/360;b=-2.14251290749072*pi/360;c=?-75.7498049314083*pi/360;
%%????????q（41）=[cos（a）*cos（b）*cos（c）+sin（a）*sin（b）*sin（c）;
%%????????????????sin（a）*cos（b）*cos（c）-cos（a）*sin（b）*sin（c）;
%%????????????????cos（a）*sin（b）*cos（c）+sin（a）*cos（b）*sin（c）;
%%????????????????cos（a）*cos（b）*sin（c）-sin（a）*sin（b）*cos（c）];
%%A矩陣為給定觀測數據，依次為GPS周秒、Gx、Gy、Gz（陀螺三軸輸出）、Ax、Ay、Az（加速度計三軸輸出）
%%C矩陣為純慣導參考結果，依次為GPS周秒、緯度、經度、高度、北向速度、東向速度、垂向速度、橫滾角、俯仰角、航向角
%%解算順序：速度解算——姿態解算——位置解算
??q=zeros（4720201）;????%%q表示從b系到n系的姿態四元數，初始狀態已知
??q（1:41）=[0.789216199658865;-0.0114037805155068;-0.0148154631808315;-0.613830795933790];
??m=zeros（3720201）;????%%m表示姿態角，依次為橫滾角、俯仰角、航向角
??m（1:31）=[0.000188410185582918;-0.0373939045021898;-1.32208350379651];???%%單位為弧度
??q1=zeros（4720200）;???%%q1表示從b系上一時刻到b系當前時刻的姿態四元數
??q2=zeros（4720200）;???%%q2表示從n系上一時刻到n系當前時刻的姿態四元數
??Cb=zeros（3720200*3）;?%%方向余弦矩陣
??Cb（:1:3）=[0.2459845120280550.969228320581583-0.00938522375454779;-0.9685525114201840.2461634155064890.0361884717532193;0.03738519043649890.0001882784724509300.999300911681383];
??v=zeros（3720201）;????%%v表示載體在n系中的地速，依次為北向速度、東向速度、垂向速度
??v（:1）=[0;0;0];
??g=zeros（1720201）;????%%重力
??g（11）=9.788297073;???%%初始時刻重力
??n=zeros（3720201）;????%%n表示每一歷元位置，依次為經度、緯度、大地高
??n（:1）=[1.97870359886305;0.403823724320167;2.175];??%%單位分別為弧度、弧度、米
??%%主體程序
??for?i=2:720201
??????%%姿態解算
??????k=[A（i2）;A（i3）;A（i4）]+cross（[A（i-12）;A（i-13）;A（i-14）][A（i2）;A（i3）;A（i4）]）/12;?????%%b系更新的矩陣
??????q1（:i-1）=[cos（norm（k）/2）;sin（norm（k）/2）*k/norm（k）];
??????Rn=6378137/sqrt（1-0.00669437999013*sin（n（2i-1））.^2）;???%%卯酉圈上個歷元曲率半徑
??????Rm=6378137*（1-0.00669437999013）/（（1-0.00669437999013*sin（n（2i-1））.*sin（n（2i-1）））.^1.5）;??%%%%子午圈上個歷元曲率半徑
??????We=[v（2i-1）/（Rn+n（3i-1））;-v（1i-1）/（Rm+n（3i-1））;-v（2i-1）*tan（n（2i-1））/（Rn+n（3i-1））];
??????Wi=[7.292115*10^-5*cos（n（2i-1））;0;-7.292115*10^-5*sin（n（2i-1））];
??????p=（We+Wi）*（A（i1）-A（i-11））;???%%n系更新的矩陣
??????q2（:i-1）=[cos（norm（p）/2）;-sin（norm（p）/2）*p/norm（p）];
??????S=[q2（1i-1）-q2（2