kalman_filter2 北航研究生课程卡尔曼滤波与组合导航第2题

北航研究生课程卡尔曼滤波与组合导航第2题，静基座初始对准

function [Att]=cnb2att（cnb）
%本函数完成从姿态矩阵到姿态角的计算
    %俯仰角计算
    theta=asin（cnb（23））;     
    %横滚角计算
    if cnb（33）>0
        gama=atan（-cnb（13）/cnb（33））;
    elseif cnb（33）<0
        if cnb（13）>0
            gama=atan（-cnb（13）/cnb（33））-pi;
        elseif cnb（13）<0
            gama=atan（-cnb（13）/cnb（33））+pi;
        end
    elseif cnb（33） == 0
        if cnb（13）>0
            gama=-pi/2;
        elseif cnb（13）<0
            gama=pi/2;
        end
    end
    % 航向角计算
    if cnb（22）>0
        if cnb（21）>0
            psai=atan（-cnb（21）/cnb（22））+2*pi;
        elseif cnb（21）<0
            psai=atan（-cnb（21）/cnb（22））;
        end
    elseif cnb（22）<0
        if cnb（21）>0
            psai=atan（-cnb（21）/cnb（22））+pi;
        elseif cnb（21）<0
            psai=atan（-cnb（21）/cnb（22））+pi;
        end
    elseif cnb（22）==0
        if cnb（21）>0
            psai=3*pi/2;
        elseif cnb（21）<0
            psai=pi/2;
        end
    end
    Att=[theta;gama;psai];

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     文件        1063  2013-04-10 14:45  cnb2att.m
     文件         383  2007-07-02 10:52  cnb2q.m
     文件         333  2013-04-11 16:22  Coarse_Alignment.m
     文件    22292852  2013-03-27 09:48  imu_data-2013年03月26日20时04分55秒.dat
     文件       11197  2013-04-13 11:04  Kalman_Filter2.m
     文件        8721  2013-04-11 22:19  Kalman_Filter2_1.m
     文件         357  2007-06-06 14:56  q2cnb.m
     文件         229  2013-04-11 16:22  Slide_avt.m