EKF UKF PF计算单IMU姿态

function [roll pitch yaw zwx zwy zwz] = CAL_ATT_EKF（carAccAccXAccYAccZGyrXGyrYGyrZNstNrDeltaT）
    persistent rollLast pitchLast yawLast;
    persistent GyrOffsetX GyrOffsetY GyrOffsetZ;
    %persistent carSpdLast;
    persistent P;
%     persistent FlagCounter;
%     persistent smthAccX smthAccY smthAccZ;
    g = 9.81;
    stDim = 5;%状态维度
    viewDim = 3;%观测维度
    verifyFlag = 1;%观测量是否参与矫正标志位
    %初始化
    if isempty（rollLast） || isempty（pitchLast） || isempty（GyrOffsetX） || isempty（GyrOffsetY） || isempty（GyrOffsetZ）
        rollLast = 0; pitchLast = 0; yawLast=0; GyrOffsetX=0; GyrOffsetY=0; GyrOffsetZ=0;carSpdLast=0;FlagCounter=0;
        smthAccX = 0; smthAccY = 0; smthAccZ = 0; 
    end
    if isempty（P）
        %P = diag（[0.01  0.01 0.01 0.01 0.01]）;
        P = diag（[0.01 0.01 0.001 0.001 0.001]）;
    end
    %求出观测量矫正标志位
%      FlagCounter = FlagCounter+1;
%      FlagCounter = mod（FlagCounter10）;
%      if FlagCounter > 0
%         verifyFlag = 0;
%      end
%     if curspd>1e-6 && abs（curspd - carSpdLast）>1e-2
%         verifyFlag = 0;
%     end
%     if curspd<1e-6
%          GyrOffsetX=0.99*GyrOffsetX + 0.01*GyrX; 
%          GyrOffsetY=0.99*GyrOffsetY + 0.01*GyrY; 
%          GyrOffsetZ=0.99*GyrOffsetZ + 0.01*GyrZ;   
%     end
%     smthAccX = 0.9 * smthAccX + 0.1 * AccX;
%     smthAccY = 0.9 * smthAccY + 0.1 * AccY;
%     smthAccZ = 0.9 * smthAccZ + 0.1 * AccZ;
    
    F = zeros（stDim6）;
    F（11） = 1;
    F（12） = sin（rollLast）*tan（pitchLast）;
    F（13） = cos（rollLast）*tan（pitchLast）;
    F（22） = cos（rollLast）;
    F（23） = -sin（rollLast）;
    %F（32） = -sin（rollLast）*sec（pitchLast）;
    %F（33） = cos（rollLast）*sec（pitchLast）;
    W = [（GyrX-GyrOffsetX） （GyrY-GyrOffsetY） （GyrZ-GyrOffsetZ） 0 0 0]‘;   
    %求雅可比矩阵，对应非线性系统线性化时F、GW和H
    wx = GyrX;wy = GyrY;wz = GyrZ;zwx = GyrOffsetX;zwy = GyrOffsetY;zwz = GyrOffsetZ;
    fai = rollLast;theta=pitchLast;psi=yawLast;
    YF = eye（stDim） + ...
     DeltaT*[     cos（fai）*tan（theta）*（wy - zwy） - sin（fai）*tan（theta）*（wz - zwz）               cos（fai）*（tan（theta）^2 + 1）*（wz - zwz） + sin（fai）*（tan（theta）^2 + 1）*（wy - zwy）  -1 -sin（fai）*tan（theta） -cos（fai）*tan（theta）;
                              - cos（fai）*（wz - zwz） - sin（fai）*（wy - zwy）                                                                                             0   0            -cos（fai）             sin（fai）;
                                                                        0                                                                                             0   0                    0                    0;
                                                                        0                                                                                             0   0                    0                    0;
                                                                        0