dynmallg.m

自己编写的鲁棒滤波算法的应用

function [StP,VtP,PsP]=DynmAllg(WibbM,FbM,St0,Ps0,Vt0,L,ts,VtG,StG,MM)
%***************************************************************************
% ********************  动机座对准  ****************************************
%***************************************************************************
%-----------------------------------------------------------------------常值
% G -- 重力加速度;   wie -- 地球转动角速度;   Re -- 地球半径;
G  = [0  0  -9.8]';   wie = 0.000072921;      Re  = 6378137;
%-----------------------------------------------------------------------时间
% xs -- 每个采样周期解算3次;      PltLth -- 绘图矩阵长度;
% ArryLth -- 解算矩阵长度;        L -- 圆整后时间;
xs = 6;                PltLth  = fix(L/ts+1);
L  = (PltLth-1)*ts;    ArryLth = xs*(PltLth-1) + 1;
%---------------------------------------------------------------------初始化
PsP = zeros(3,PltLth);    VtP = zeros(3,PltLth);     StP = zeros(2,PltLth);
PsP(:,1) = Ps0;           VtP(:,1) = Vt0;            StP(:,1) = St0;
%--------------------------------------------------------------解算变量初始化
% Ps0 -- 初始姿态,    Vt0 -- 初始速度,        St0 -- 初始位置
% CbnJ -- 初始化方向余弦阵    QJ -- 初始化四元数
% Dw -- 陀螺零偏      Dug -- 加速度计零偏     DeA -- 失准角
PsJ = Ps0;   VtJ = Vt0;  StJ = St0;  LamdaJ = St0(1);  PheJ   = St0(2);
StM = St0;  LamdaM = St0(1);  PheM   = St0(2);
CbnJ = GetCbn( Ps0 );         QJ  = GetQn ( CbnJ);     log = 0; log1 = 0;DeU=0;
Dw   = [0  0  0]';    Dug = [0  0  0]';       DeA = [0  0  0]';
%-------------------------------------------------------------Kalman滤波初始化
% XkP -- 失准角估计存储;            PkP -- 估计状态协方差存储
%KL = 12; tkn = 33;  Ts =ts*tkn;  FA = zeros(KL,KL);
%XkP = zeros(9,fix(PltLth/tkn));     PkP = zeros(KL,fix(PltLth/tkn));
%w1 = zeros(3,PltLth);   w2 = zeros(3,PltLth);
%---------------------------------------------------------------------导航解算
for t = 1 : xs : ArryLth-xs
    tp=(t+5)/xs;                       %导航解算周期
    %--------------------------------------------------------------哥氏速度更新
    WentJ = [-VtJ(2)/Re, VtJ(1)/Re, VtJ(1)*tan(PheJ)/Re]';
    WietJ = [0,  wie*cos(PheJ),  wie*sin(PheJ)]';
    WintJ = WentJ+WietJ;
    %-------------------------------------------------------------陀螺测量值更新
    WibbJ = WibbM( : , t:t+xs) - Dw*ones(1,xs+1);    % Dw -- 陀螺零偏
    %------------------------------------WnbbJ = WibbJ-WinbJ = WibbJ-CbnJ'*WintJ
    WnbbJ = WibbJ - CbnJ'*WintJ*ones(1,xs+1);
    %-----------------------------------Data=[Data1;Data2;Data3]------角增量提取
    Wnbb1= WnbbJ(:,1:2:5);   Wnbb2= WnbbJ(:,3:2:7);   Wnbb3= WnbbJ(:,2:2:6);
    Deta  = ( Wnbb1 + Wnbb2 + 4*Wnbb3 )*ts/(xs*3);   %辛普森提取
    %Deta  = ( Wnbb1 + Wnbb2 )*ts/(xs);              %梯形法提取
    %---三子样法求等效旋转矢量---------------------------------------四元数更新    
    [QJ, CbnJ] = ThrSmpl(QJ, Deta(:,1), Deta(:,2), Deta(:,3));
    %----------------------------------------------------------------姿态角更新
    [PsJ] = GetPs(CbnJ);    PsP(:,tp+1) = PsJ;  % 姿态装订
    %-----------------------------------------------------------------加速度更新
    FbJ = (FbM(:,t)+FbM(:,t+1)+FbM(:,t+2)+FbM(:,t+3)+FbM(:,t+4)+FbM(:,t+5))/6 - Dug;
    FtJ = CbnJ*FbJ;                             % Dug -- 加速度零偏补偿
    AtJ = FtJ - cross((2*WietJ+WentJ),VtJ) + G;
    %-------------------------------------------------------------------速度更新
    VtJ = VtJ + AtJ*ts;     VtP(:,tp+1) = VtJ;  % 速度装订
    %-------------------------------------------------------------------位置更新 
    LamdaJ = LamdaJ+VtJ(1)*ts*sec(PheJ)/Re;      PheJ= PheJ+VtJ(2)*ts/Re;
    StJ = [LamdaJ,PheJ]';   StP(:,tp+1) = StJ;  % 经纬度装订
   %----------------------------------------------------------------------------
end
%--------------------------------------------------------------------------------