对test_SINS_GPS_EKF_153中ekf的理解

最新推荐文章于 2024-09-06 14:10:42 发布

水下的史莱姆

最新推荐文章于 2024-09-06 14:10:42 发布

阅读量205

点赞数 2

文章标签： matlab

本文链接：https://blog.youkuaiyun.com/weixin_49313215/article/details/138589382

版权

%程序中确实演示EKF的流程，但是好像还是用的KF153中的系统模型，
%所以个人感觉最后还是用了EKF中的判别，判别为线性系统，所以解算内核还是线性KF

%ekf函数

function kf = ekf(kf, yk, TimeMeasBoth)
% Extended Kalman filter for nonlinear system.
%
% Prototype: kf = ekf(kf, yk)
% Inputs: kf - filter structure array
%         yk - measurement vector
%         TimeMeasBoth - described as follows,
%            TimeMeasBoth='T' (or nargin==1) for time updating only, 
%            TimeMeasBoth='M' for measurement updating only, 
%            TimeMeasBoth='B' (or nargin==2) for both time and 
%                             measurement updating.
% Output: kf - output filter structure array
%
% See also  ekfJcb, ukf, ckf, kfupdate.

% Copyright(c) 2009-2022, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 27/09/2012, 04/03/2022
    if nargin==1;
        TimeMeasBoth = 'T';
    elseif nargin==2
        TimeMeasBoth = 'B';
    end
%% KF流程前两步
    if TimeMeasBoth=='T' || TimeMeasBoth=='B'
        if isfield(kf, 'fx')  % nonliear state Jacobian matrix
            %如果kf结构体中有fx（fk中存储状态转移非线性函数），
            %则认为状态方程为非线性模型，按非线性系统进行状态一步预测
            [kf.Phikk_1, kf.xkk_1] = ekfJcb(kf.fx, kf.xk, kf.px);
            %状态转移函数对状态取雅克比，得状态一步转移矩阵
            if isempty(kf.xkk_1), kf.xkk_1 = kf.Phikk_1*kf.xk; end
        else
            kf.xkk_1 = kf.Phikk_1*kf.xk;
            %如果kf结构体中没有fx，则认为状态方程为线性模型，按线性系统进行状态一步预测
        end
        kf.Pxkk_1 = kf.Phikk_1*kf.Pxk*kf.Phikk_1' + kf.Gammak*kf.Qk*kf.Gammak';
        if TimeMeasBoth=='T'    % time updating only
            kf.xk = kf.xkk_1; kf.Pxk = kf.Pxkk_1;
            return;
        end
    end
%% KF流程后三步（及中间量计算）
    if TimeMeasBoth=='M' || TimeMeasBoth=='B'
        if TimeMeasBoth=='M'    % meas updating only
            kf.xkk_1 = kf.xk; kf.Pxkk_1 = kf.Pxk;
        end
        if isfield(kf, 'hx')  % nonliear measurement Jacobian matrix 
            %如果kf结构体中有hx，则认为观测方程为非线性模型，按非线性系统进行观测更新和卡尔曼增益计算
            [kf.Hk, kf.ykk_1] = ekfJcb(kf.hx, kf.xkk_1, kf.py);
            %状态观测函数对状态一步预测取雅克比，得观测矩阵
            if isempty(kf.ykk_1), kf.ykk_1 = kf.Hk*kf.xkk_1; end
        else
            kf.ykk_1 = kf.Hk*kf.xkk_1;
        end
        kf.Pxykk_1 = kf.Pxkk_1*kf.Hk';    kf.Pykk_1 = kf.Hk*kf.Pxykk_1 + kf.Rk;
        % filtering
        kf.Kk = kf.Pxykk_1*kf.Pykk_1^-1;
        kf.xk = kf.xkk_1 + kf.Kk*(yk-kf.ykk_1);
        kf.Pxk = kf.Pxkk_1 - kf.Kk*kf.Pykk_1*kf.Kk';  kf.Pxk = (kf.Pxk+kf.Pxk')/2;
    end