• 大小: 4KB
    文件类型: .m
    金币: 1
    下载: 0 次
    发布日期: 2021-05-16
  • 语言: Matlab
  • 标签: ekf。matlab  

资源简介

这是关于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

评论

共有 条评论

相关资源