扩展卡尔曼滤波姿态解算

潮涌M语言实现，采用三轴角速率陀螺三轴加速度计结合扩展卡尔曼滤波实现姿态结算

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

%load inital values（载入初始值）
ekf_init;
%timesync_init;（时间初始化）

%load logged data（载入存入数据）
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
%%国际标准地磁场磁场谐波系数
g10=-0.29615;   %高斯系数
g11=-0.01728;   %高斯系数
h11=0.05186;    %高斯系数

%change la input variable to field equation parameters
%将输入变量装换为场方程参数
cel=（90-la）*pi/180;  % coelevation [rad] 海拔高度单位为弧度
elo=lo*pi/180;       %east longitude [rad]（=azimuth=longitude） 东经度（=方位角=经度）
r=6371e3+h;          % geocentric distance [m] 地心距


%dipole modle（return magnetic field in navigation axes ）
%磁偶极子模式（返回导航坐标系的磁场）
% 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 计算重力场
%%重力场+机体加速度
g_0=9.81;            %海平面重力加速度
g_n=[0 0 -g_0]‘;     %临时重力场模型，与高度相关  
x_dd_n=[0 0 0]‘;     %机体加速度，假设为0
a_n=x_dd_n+g_n;    
lambda_a=[0.2 0.2 0.2]‘;   %%加速度计偏差矢量
% scale_a=[1 -1 -1]‘;                        %%加速度计矢量缩放因子
 

%%磁力计校正值
%%Honeywell 磁力计 （理论上的值）
 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，因为an为一列向量，不存在逆，因此要用下面方法构造可逆矩阵求解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中提取三个姿态角
    att（1i）=atan（T_nb（23））/T_nb（33）*180/pi;     %%  滚转角（角度）
    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;
    

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件     226224  2008-12-20 00:46  姿态融合-（欧拉描述）\alt.mat

     文件       5213  2008-12-16 15:19  姿态融合-（欧拉描述）\attref.m

     文件     226224  2008-12-20 00:46  姿态融合-（欧拉描述）\Ax.mat

     文件     226224  2008-12-20 00:46  姿态融合-（欧拉描述）\Ay.mat

     文件     226224  2008-12-20 00:46  姿态融合-（欧拉描述）\Az.mat

     文件     226224  2009-01-14 22:47  姿态融合-（欧拉描述）\a_mlog.mat

     文件     226224  2009-01-14 22:47  姿态融合-（欧拉描述）\B_mlog.mat

     文件      32432  2009-01-08 13:05  姿态融合-（欧拉描述）\corrP.mat

     文件      32432  2009-01-08 13:05  姿态融合-（欧拉描述）\corrx.mat

     文件        543  2008-12-17 09:50  姿态融合-（欧拉描述）\Data1.m

     文件       1643  2008-12-16 17:16  姿态融合-（欧拉描述）\ekf.m

     文件       1272  2008-12-16 15:32  姿态融合-（欧拉描述）\ekf_init.m

     文件        266  2008-12-08 13:54  姿态融合-（欧拉描述）\f.m

     文件       1106  2008-12-06 15:14  姿态融合-（欧拉描述）\F_xa.m

     文件         31  2008-12-08 13:10  姿态融合-（欧拉描述）\g.m

     文件     226224  2008-12-20 00:46  姿态融合-（欧拉描述）\Gama.mat

     文件     226224  2009-01-14 22:47  姿态融合-（欧拉描述）\g_mlog.mat

     文件       1309  2008-12-15 17:25  姿态融合-（欧拉描述）\h_ins.m

     文件       3296  2008-12-15 21:27  姿态融合-（欧拉描述）\H_xa.m

     文件      32432  2009-01-08 13:05  姿态融合-（欧拉描述）\inproc.mat

     文件     226224  2008-12-20 00:46  姿态融合-（欧拉描述）\Lat.mat

     文件       2682  2008-12-15 17:33  姿态融合-（欧拉描述）\load_data.m

     文件     226224  2008-12-20 00:46  姿态融合-（欧拉描述）\Lon.mat

     文件       2936  2008-12-15 21:54  姿态融合-（欧拉描述）\main.m

     文件       1021  2008-12-06 16:07  姿态融合-（欧拉描述）\mg_fields.m

     文件     226224  2008-12-20 00:46  姿态融合-（欧拉描述）\p.mat

     文件       1721  2009-01-08 12:26  姿态融合-（欧拉描述）\plot1.asv

     文件       1808  2009-01-08 12:41  姿态融合-（欧拉描述）\plot1.m

     文件        677  2008-12-20 01:31  姿态融合-（欧拉描述）\plot2.asv

     文件      32432  2009-01-08 13:05  姿态融合-（欧拉描述）\predP.mat

............此处省略27个文件信息