ekf的滤波程序，以及在组合导航中的应用

function EKF4（）    
%   EKF  static base alignment
%    ZMH
    clear all

    L = 34*pi/180;
    wie = 7.2921158e-5;
    g = 9.8;
    sl = sin（L）;
    cl = cos（L）;
    
    gyrobias = 0.02*pi/180/3600;
    accbias = 1e-4*9.8;
    gyrostoc = 0.01*pi/180/3600;
    accstoc= 0.5e-4*9.8/2;

    P = diag（[0.1^20.1^2（1*pi/180）^2（1*pi/180）^2（1*pi/180）^2]）;


    sQ = diag（[accstocaccstocgyrostocgyrostocgyrostoc]）;
    Q = diag（[accstoc^2accstoc^2gyrostoc^2gyrostoc^2gyrostoc^2]）;
    sR = diag（[0.10.1]）;
    R = diag（[0.010.01]）;
    
    H = [diag（[11]）zeros（23）];

    xest = [0.10.1（1*pi/180）（1*pi/180）（1*pi/180）]‘;    %estimate
    x = zeros（51）;    dxpre = zeros（51）;dxest = zeros（51）;

    xobs = zeros（51）;
    dxobs = multivariate_gauss（x Q 1）;
    z = H*dxobs;
    dzobs = multivariate_gauss（z R 1）;


NN = 1000;



for i=1:NN
    
    F =eye（5） + numerical_Jacobian（xobs @predict_model）;

    dxpre = F*dxest;
    P = F*P*F‘ + Q;
   
    % update
    [dxestP] = update（dxpre P dzobs R H 3）; % the end parameter specifies which KF function to use.
    

    xest = xobs + dxest;
    
    dxobs = F*dxobs;
    dxobs = multivariate_gauss（dxobs  Q 1）;
    xobs = xobs + predict_model（xobs）;
    

    dz = observe_model（dxobsH）;
	dzobs = multivariate_gauss（dz R 1）;  

    % plots
    xout（:i） = xest;
    Pout（1i） = sqrt（P（33））;
    Pout（2i） = sqrt（P（44））;
    Pout（3i） = sqrt（P（55））;
end

    i=1:NN;
    subplot（311）
    plot（ixout（1i）‘r‘ixout（2i）‘b‘）;
        hold on
    subplot（312）
    plot（ixout（3i）‘r‘ixout（4i）‘b‘ixout（5i）‘g‘）
        hold on
    subplot（313）
    plot（iPout（1i）‘r‘iPout（2i）‘b‘iPout（3i）‘g‘）
        hold on