model_v_a_linear.m

惯性导航系统传递对准系统模型程序

function [F,G,H,Z]=Model_V_A_linear(modelpara)
v_s = modelpara.v_s;%速度＋全姿态线性模型计算程序。输入模型参数，输出模型的F、G、H、Z阵，从而得到系统的状态空间模型。在此模型中考虑两种情况：
%1、不考虑杆臂效应的情形；2、考虑杆臂效应的影响。把主子惯导系统的速度差值和误差角作为观测量，组成矩阵A和矩阵B，再进行离散化处理得到F阵和G阵，H
%阵。在此函数中调用了dcm_eulr()函数，进行方向余弦和欧拉角的转换计算。
v_m = modelpara.v_m;
T_m = modelpara.T_m;
fb_s = modelpara.fb_s;
T_s = modelpara.T_s;
wnbb_s = modelpara.wnbb_s;
wnbb_m = modelpara.wnbb_m;
wien_s=modelpara.wien_s;
wenn_s=modelpara.wenn_s;
wien_m=modelpara.wien_m;
wenn_m=modelpara.wenn_m;
wimn_m = wien_m+wenn_m+T_m*wnbb_m;
wimn_s = wien_s+wenn_s+T_s*wnbb_s;
step = modelpara.step;
switch modelpara.error
    case 'Consider'
        ang_ms=dcm_eulr(T_s'*T_m);
        Z = [v_s(1:2,1) - v_m(1:2,1)
            ang_ms       ];
        A2=[T_s(1,2)*fb_s(3)-T_s(1,3)*fb_s(2) -T_s(1,1)*fb_s(3)+T_s(1,3)*fb_s(1) T_s(1,1)*fb_s(2)-T_s(1,2)*fb_s(1)
            T_s(2,2)*fb_s(3)-T_s(2,3)*fb_s(2) -T_s(2,1)*fb_s(3)+T_s(2,3)*fb_s(1) T_s(2,1)*fb_s(2)-T_s(2,2)*fb_s(1)];
        A1=-A2;
        A3=T_s(1:2,1:2);
        A9=[ 0 -wnbb_s(3) wnbb_s(2)
            wnbb_s(3) 0 -wnbb_s(1)
            -wnbb_s(2) wnbb_s(1) 0];
        A8=-A9;
        A10=eye(3);
        A=[zeros(2,2) A1 A2 A3 zeros(2,3)
            zeros(3,2) A8 A9 zeros(3,2) A10
            zeros(8,13)        ];
        B=[A3 zeros(2,3)
            zeros(3,2)  A10
            zeros(8,5) ];
        [F,G] = c2d(A,B,step);
        H = [eye(5) zeros(5,8)];

    case 'LevelArm_Consider'
        ang_ms=dcm_eulr(T_s'*T_m);
        Z = [v_s(1:2,1) - v_m(1:2,1)
            ang_ms       ];
        A2=[T_s(1,2)*fb_s(3)-T_s(1,3)*fb_s(2) -T_s(1,1)*fb_s(3)+T_s(1,3)*fb_s(1) T_s(1,1)*fb_s(2)-T_s(1,2)*fb_s(1)
            T_s(2,2)*fb_s(3)-T_s(2,3)*fb_s(2) -T_s(2,1)*fb_s(3)+T_s(2,3)*fb_s(1) T_s(2,1)*fb_s(2)-T_s(2,2)*fb_s(1)];
        A1=-A2;
        A6=-SymMatrix(wien_m)*SymMatrix(wimn_m)*T_m;
        A3=T_s(1:2,1:2);
        A9=[ 0 -wnbb_s(3) wnbb_s(2)
            wnbb_s(3) 0 -wnbb_s(1)
            -wnbb_s(2) wnbb_s(1) 0];
        A8=-A9;
        A10=eye(3);
        A=[zeros(2,2) A1 A2 A3 zeros(2,3)  A6(1:2,:)
            zeros(3,2) A8 A9 zeros(3,2) A10 zeros(3,3)
            zeros(11,16)        ];
        B=[A3 zeros(2,3)
            zeros(3,2)  A10
            zeros(11,5) ];
        [F,G] = c2d(A,B,step);
        H = [eye(5) zeros(5,11)];
end