捷联惯导解算

捷联惯导解算程序，有详细的代码标注和参考文献说明，主要参考亲永元的《组合导航》的书，提供给大家一起学习。

function  [attqbn] = align_kalman（timeimugyracclenpos）
%kalman滤波方法初始对准
global glv
%[RmRng]=earth（pos（1））;
[RmRng]=earth（pos）;
% gn=[0;0;-g];
%att = coarse_align（acc gyr pos（1） gn）; % 粗对准
att = Allign_coarse（accgyrglv.time_coarse*glv.tgpos（1））;     % 粗对准             pos（1）
qbn0 = att2quat（att） ;                            % 姿态转四元数，b系到n系
wnie = glv.wie*[0; cos（pos（1））; sin（pos（1））];                                     %pos（1）2
% 滤波器参数参见《捷联惯导系统静基座初始对准精度分析及仿真》
Qt = diag（[glv.eb; 0;0]）^2;             % 系统噪声方差矩阵 （陀螺漂移）陀螺仪零偏
Rk = diag（[glv.db（1）;glv.db（2）]）^2;        % 观测噪声方差矩阵（加速度计漂移）加速度计零偏
xk = zeros（51）;                     % 状态向量：姿态角误差、北向陀螺漂移、天向陀螺漂移
Pk = diag（[[1;1;5]*glv.deg; [0.5;0.5]*glv.dph]）^2;     %协方差矩阵deg角度dph度每小时
Phi = [ 0 wnie（3） -wnie（2）   0 0               %连续状态方程的状态转移矩阵
      -wnie（3） 0 0    -1 0 
      wnie（2） 0 0     0 -1 
      zeros（25） ];                 
Hk = [ 0 g  0 0 0
      -g 0  0 0 0 ];             % 观测矩阵
  qbn=qbn0‘;
for i = （glv.time_coarse*glv.t）:len-1
    Ts = timeimu（i+1） - timeimu（i）;
    Phikk_1 = eye（5）+Phi*Ts;            % 状态转移矩阵离散化
    wbe = gyr（i:）‘;
    fbe = acc（i:）‘;
    qbn = qmul（qbn rv2q（（wbe-qmulv（qconj（qbn）wnie））*Ts ））;  % 姿态更新（循环）单子样，               qmul四元数相乘，qconj共轭四元数
    fn = qmulv（qbn fbe）;             %比力转化为导航坐标系                                         qumlv四元数乘向量，rv2q等效旋转矢量转换为四元数，q2rv四元数转换为等效矩阵
    zk = fn（1:2）;  % 以水平比力为观测量 
    [xk Pk] = kalman（Phikk_1 Qt*Ts xk Pk Hk Rk zk）;   % 开环卡尔曼滤波
end
bn = qdelphi（qbnxk（1:3））;                        %姿态更新四元数微分方程
att = q2att（qbn）;                                  %四元数转换为姿态角

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

     文件       1882  2014-12-03 19:57  严恭敏捷联惯导程序\align_kalman.asv

     文件       1909  2014-12-04 18:42  严恭敏捷联惯导程序\align_kalman.m

     文件        538  2014-04-18 09:46  严恭敏捷联惯导程序\Allign_coarse.asv

     文件        536  2014-04-18 09:50  严恭敏捷联惯导程序\Allign_coarse.m

     文件        428  2013-10-10 03:13  严恭敏捷联惯导程序\att2quat.asv

     文件        408  2012-07-30 17:30  严恭敏捷联惯导程序\att2quat.m

     文件        154  2012-07-30 17:21  严恭敏捷联惯导程序\Cnb2atti.m

     文件       1182  2014-04-18 16:22  严恭敏捷联惯导程序\cnscl.asv

     文件       1196  2014-04-18 16:24  严恭敏捷联惯导程序\cnscl.m

     文件        394  2014-04-09 23:02  严恭敏捷联惯导程序\earth.asv

     文件        395  2014-12-03 19:56  严恭敏捷联惯导程序\earth.m

     文件          0  2013-10-21 23:19  严恭敏捷联惯导程序\glv.eb

     文件       1575  2014-04-18 23:42  严恭敏捷联惯导程序\glvs.asv

     文件       1575  2014-04-18 23:43  严恭敏捷联惯导程序\glvs.m

     文件     605701  2014-04-18 10:13  严恭敏捷联惯导程序\imu.mat

     文件        604  2014-04-15 14:13  严恭敏捷联惯导程序\kalman.asv

     文件        604  2014-04-15 14:17  严恭敏捷联惯导程序\kalman.m

     文件        403  2013-10-10 03:17  严恭敏捷联惯导程序\normQ.asv

     文件        393  2012-03-27 20:56  严恭敏捷联惯导程序\normQ.m

     文件        248  2012-08-01 16:26  严恭敏捷联惯导程序\position.m

     文件    1695012  2015-03-03 21:28  严恭敏捷联惯导程序\pp；.mat

     文件        100  2013-09-12 17:33  严恭敏捷联惯导程序\q2att.m

     文件        455  2012-07-30 10:43  严恭敏捷联惯导程序\q2cbn.m

     文件         75  2014-04-15 09:53  严恭敏捷联惯导程序\qconj.asv

     文件         75  2014-04-15 09:56  严恭敏捷联惯导程序\qconj.m

     文件         65  2012-07-27 16:50  严恭敏捷联惯导程序\qdelphi.m

     文件        342  2013-11-02 20:34  严恭敏捷联惯导程序\qmul.asv

     文件        342  2013-11-02 20:39  严恭敏捷联惯导程序\qmul.m

     文件        145  2014-04-15 14:33  严恭敏捷联惯导程序\qmulv.asv

     文件        147  2014-04-15 14:33  严恭敏捷联惯导程序\qmulv.m

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