clc
close?all
clear?all
format?long
%%?數據讀取
load（‘IMU.dat‘）;
load（‘GPS.dat‘）;?
f_INSc=IMU（1:end-16:8）‘;
wib_INSc=IMU（1:end-13:5）‘*pi/180/3600;
P_GPS=GPS（2:end3:5）‘;
V_GPS=GPS（2:end6:8）‘;
n=length（IMU）;N=length（GPS）;%數據個數
%%?初值設定
L=34.6521606250000*pi/180;????????????????????????????????%初始緯度
J=109.249623717000*pi/180;????????????????????????????%初始經度
h=362.269000000000;??????????????????????????????????????????????%初始高度（m）
Wie=7.292115147e-5;????????????????????????????????%地球自轉角速度（rad/s）
hangxiang=303.10881*pi/180;????????????????????????????????????%初始航向角
fuyang=0.25516*pi/180;????????????????????????????????????%初始俯仰角
henggun=1.76037*pi/180;????????????????????????????????????%初始橫滾角
T=0.01;%采樣周期（s）
Re=6378245?;???????????????????????????????????
e=1/298.3?;????????????????????????????????????
Vx=0;???????????????????????????????????%初始東向水平速度（m/s）
Vy=0.00900000000000000;?????????????????%初始北向水平速度（m/s）
Vz=0.0350000000000000;??????????????????%初始天向速度（m/s）
g0=9.7803267714;???????????????????????????????
gk1=0.00193185138639;????????????????????????????
gk2=0.00669437999013;
W=zeros（3n-1）;????????????????????????????????????%用于記錄實時三軸指令角速度
V=zeros（3n-1）;????????????????????????????????????%用于記錄實時東北天速度
attitude=zeros（3n-1）;?????????????????????????????%姿態角
Alt=zeros（3N-1）;
pos=zeros（3N-1）;
vel=zeros（3N-1）;
q=zeros（41）;??????????????????????????????????????%四元數
q（1）=cos（hangxiang/2）*cos（fuyang/2）*cos（henggun/2）-sin（hangxiang/2）*sin（fuyang/2）*sin（henggun/2）;
q（2）=cos（hangxiang/2）*sin（fuyang/2）*cos（henggun/2）-sin（hangxiang/2）*cos（fuyang/2）*sin（henggun/2）;
q（3）=cos（hangxiang/2）*cos（fuyang/2）*sin（henggun/2）+sin（hangxiang/2）*sin（fuyang/2）*cos（henggun/2）;
q（4）=cos（hangxiang/2）*sin（fuyang/2）*sin（henggun/2）+sin（hangxiang/2）*cos（fuyang/2）*cos（henggun/2）;
PP=zeros（15N-1）;
X0=zeros（151）;
P0=eye（1515）;
X=zeros（15N-1）;
?%%?捷聯結算
for?i=1:N-1
????for?t=（i-1）*5+1:（i-1）*5+5;
????????Cnb=[q（1）^2+q（2）^2-q（3）^2-q（4）^2??2*（q（2）*q（3）+q（1）*q（4））??????2*（q（2）*q（4）-q（1）*q（3））;
??????????2*（q（2）*q（3）-q（1）*q（4））??????q（1）^2-q（2）^2+q（3）^2-q（4）^2??2*（q（3）*q（4）+q（1）*q（2））;
??????????2*（q（2）*q（4）+q（1）*q（3））??????2*（q（3）*q（4）-q（1）*q（2））??????q（1）^2-q（2）^2-q（3）^2+q（4）^2;];

????????g=g0*（1+gk1*sin（L）^2）*（1-2*h/Re）/sqrt（1-gk2*sin（L）^2）;
????????ft=Cnb‘*f_INSc（:t）*g;?????????????????????????%本體系下的比力轉換到導航系
????????Rxt=（1-e*（sin（L）^2））/Re;??????????????????????
????????Ryt=（1+2*e-3*e*（sin（L）^2））/Re;?????????????????
????????Dvx=ft（1）+2*Wie*sin（L）*Vy+Vx*Vy*Rxt*tan（L）-（2*Wie*cos（L）+Vx*Rxt）*Vz;?%東向加速度
????????Dvy=ft（2）-2*Wie*sin（L）*Vx-Vx^2*Rxt*tan（L）-Vy*Ryt*Vz;?????????????????%北向加速度
????????Dvz=ft（3）+（2*Wie*cos（L）+Vx*Rxt）*Vx+Vy*Ryt*Vy-g;??????????????????????%天向加速度
????????Vx=Vx+Dvx*T;???????????????????????????????????%東向速度
????????Vy=Vy+Dvy*T;???????????????????????????????????%北向速度
????????Vz=Vz+Dvz*T;???????????????????????????????????%北向速度
????????V（:t）=[VxVyVz]‘;????????????????????????????%實時速度記錄
???????

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????文件?????8099809??2020-09-28?17:39??GPS.dat
?????文件????68403987??2020-09-28?17:39??IMU.dat
?????文件???????10299??2020-09-28?17:39??ins-gps.m