clc;
clear;
format?short?e;
disp（‘main.m:?control?postprocessing?logged?data?\n‘）

%load?inital?values（載入初始值）
ekf_init;
%timesync_init;（時(shí)間初始化）

%load?logged?data（載入存入數(shù)據(jù)）
load_data;
for?i=1:404
a_m=a_mlog（i2:4）‘;
%w_mlog（i2:4）?
B_m=B_mlog（i2:4）‘;

a=6378.140e3;??%equatorial?radius?of?earth（wertzp819）?地球赤道半徑
g_0=9.81;??????%gravitational?acceleration?at?sea?level?海平面重力加速度

%%?IGRF?magnetic?field?harmonic?coefficients?see?wertz
%%國(guó)際標(biāo)準(zhǔn)地磁場(chǎng)磁場(chǎng)諧波系數(shù)
g10=-0.29615;???%高斯系數(shù)
g11=-0.01728;???%高斯系數(shù)
h11=0.05186;????%高斯系數(shù)

%change?la?input?variable?to?field?equation?parameters
%將輸入變量裝換為場(chǎng)方程參數(shù)
cel=（90-la）*pi/180;??%?coelevation?[rad]?海拔高度單位為弧度
elo=lo*pi/180;???????%east?longitude?[rad]（=azimuth=longitude）?東經(jīng)度（=方位角=經(jīng)度）
r=6371e3+h;??????????%?geocentric?distance?[m]?地心距


%dipole?modle（return?magnetic?field?in?navigation?axes?）
%磁偶極子模式（返回導(dǎo)航坐標(biāo)系的磁場(chǎng)）
%?B（1:）=2*（（a/r）^3）*（g10*cos（cel）+（g11*cos（elo）+h11*sin（elo））*sin（cel））;
%?B（2:）=（（a/r）^3）*（g10*sin（cel）-（g11*cos（elo）+h11*sin（elo））*cos（cel））;
%?B（3:）=（（a/r）^3）*（g11*sin（elo）-h11*cos（elo））;
B_n（1）=-B_m（2）;
B_n（2）=B_m（3）;
B_n（3）=-B_m（1）;

%caculate?gravity?field?計(jì)算重力場(chǎng)
%%重力場(chǎng)+機(jī)體加速度
g_0=9.81;????????????%海平面重力加速度
g_n=[0?0?-g_0]‘;?????%臨時(shí)重力場(chǎng)模型，與高度相關(guān)??
x_dd_n=[0?0?0]‘;?????%機(jī)體加速度，假設(shè)為0
a_n=x_dd_n+g_n;????
lambda_a=[0.2?0.2?0.2]‘;???%%加速度計(jì)偏差矢量
%?scale_a=[1?-1?-1]‘;????????????????????????%%加速度計(jì)矢量縮放因子
?

%%磁力計(jì)校正值
%%Honeywell?磁力計(jì)?（理論上的值）
?K_B_h=diag（[1?1?1]）;??????????



%?K_B_x=[
%?????????8077.877?-73.453?-180.367;
%?????????-123.268?-7810.493?38.309;
%?????????118.145?54.293?-8360.807];
lambda_B_x=[0.2?0.2?0.2]‘;
%?scale_x=0.48742*[-1?1?-1];

????c=a_m-lambda_a;?????????%%c=Tnb*an，因?yàn)閍n為一列向量，不存在逆，因此要用下面方法構(gòu)造可逆矩陣求解Tnb
????d_x=B_m-lambda_B_x;?????%%d_x=Tnb*B_x
????A=[c?d_x?cross（cd_x）];????????????????????????%%T_nb*B=A
????B=[a_n?B_n?cross（a_nB_n）];????????????????????%%T_nb*（an?B_x?cross（anB_x））=（Tnb*an?Tnb*B_x）
????T_nb=A*inv（B）;
????
????
????%%從矩陣Tnb中提取三個(gè)姿態(tài)角
????att（1i）=atan（T_nb（23））/T_nb（33）*180/pi;?????%%??滾轉(zhuǎn)角（角度）
????att（2i）=-asin（T_nb（13））*180/pi;??????????????%%??俯仰角（角度）
????att（3i）=atan（T_nb（12））/T_nb（11）*180/pi;?????%%??偏航角（角度）
????????B=d_x;
????%%c=[-sin（Theta）*g_0??sin（Phi）cos（Theta）*g_0??cos（Phi）cos（Theta）*g_0]‘
????Phi_z=atan（c（2）/c（3））;?????????????????
????Theta_z=asin（-c（1）/g_0）;
????
????%%Hxc=Hx*cos（Phi）+Hy*sin（Theta）*cos（Phi）-Hz*cos（cosTheta）sin（Phi）
????%%Hyc=Hy*cos（Theta）-Hz*sin（Theta）
????%%Phi=arctan（-Hyc/Hxc）
????A=cos（Theta_z）*B（1）+sin（Theta_z）*sin（Phi_z）*B（2）+sin（Theta_z）*cos（Phi_z）*B（3）;
????B=cos（Phi_z）*B（2）-sin（Phi_z）*B（3）;
????Psi_z=atan（-B/A）;
????
????
????zwaai（1:）=Phi_z*180/pi;?????????????????
????zwaai（2:）=Theta_z*180/pi;
????zwaai（3:）=Psi_z*180/pi;?

end
????
????save?att;
????save?zwaii;
????

?屬性????????????大小?????日期????時(shí)間???名稱
-----------?---------??----------?-----??----

?????文件?????226224??2008-12-20?00:46??姿態(tài)融合-（歐拉描述）\alt.mat

?????文件???????5213??2008-12-16?15:19??姿態(tài)融合-（歐拉描述）\attref.m

?????文件?????226224??2008-12-20?00:46??姿態(tài)融合-（歐拉描述）\Ax.mat

?????文件?????226224??2008-12-20?00:46??姿態(tài)融合-（歐拉描述）\Ay.mat

?????文件?????226224??2008-12-20?00:46??姿態(tài)融合-（歐拉描述）\Az.mat

?????文件?????226224??2009-01-14?22:47??姿態(tài)融合-（歐拉描述）\a_mlog.mat

?????文件?????226224??2009-01-14?22:47??姿態(tài)融合-（歐拉描述）\B_mlog.mat

?????文件??????32432??2009-01-08?13:05??姿態(tài)融合-（歐拉描述）\corrP.mat

?????文件??????32432??2009-01-08?13:05??姿態(tài)融合-（歐拉描述）\corrx.mat

?????文件????????543??2008-12-17?09:50??姿態(tài)融合-（歐拉描述）\Data1.m

?????文件???????1643??2008-12-16?17:16??姿態(tài)融合-（歐拉描述）\ekf.m

?????文件???????1272??2008-12-16?15:32??姿態(tài)融合-（歐拉描述）\ekf_init.m

?????文件????????266??2008-12-08?13:54??姿態(tài)融合-（歐拉描述）\f.m

?????文件???????1106??2008-12-06?15:14??姿態(tài)融合-（歐拉描述）\F_xa.m

?????文件?????????31??2008-12-08?13:10??姿態(tài)融合-（歐拉描述）\g.m

?????文件?????226224??2008-12-20?00:46??姿態(tài)融合-（歐拉描述）\Gama.mat

?????文件?????226224??2009-01-14?22:47??姿態(tài)融合-（歐拉描述）\g_mlog.mat

?????文件???????1309??2008-12-15?17:25??姿態(tài)融合-（歐拉描述）\h_ins.m

?????文件???????3296??2008-12-15?21:27??姿態(tài)融合-（歐拉描述）\H_xa.m

?????文件??????32432??2009-01-08?13:05??姿態(tài)融合-（歐拉描述）\inproc.mat

?????文件?????226224??2008-12-20?00:46??姿態(tài)融合-（歐拉描述）\Lat.mat

?????文件???????2682??2008-12-15?17:33??姿態(tài)融合-（歐拉描述）\load_data.m

?????文件?????226224??2008-12-20?00:46??姿態(tài)融合-（歐拉描述）\Lon.mat

?????文件???????2936??2008-12-15?21:54??姿態(tài)融合-（歐拉描述）\main.m

?????文件???????1021??2008-12-06?16:07??姿態(tài)融合-（歐拉描述）\mg_fields.m

?????文件?????226224??2008-12-20?00:46??姿態(tài)融合-（歐拉描述）\p.mat

?????文件???????1721??2009-01-08?12:26??姿態(tài)融合-（歐拉描述）\plot1.asv

?????文件???????1808??2009-01-08?12:41??姿態(tài)融合-（歐拉描述）\plot1.m

?????文件????????677??2008-12-20?01:31??姿態(tài)融合-（歐拉描述）\plot2.asv

?????文件??????32432??2009-01-08?13:05??姿態(tài)融合-（歐拉描述）\predP.mat

............此處省略27個(gè)文件信息