ekf.m

来自「一个关于卫星导航方面的ekf滤波程序」· M 代码 · 共 40 行

function [xh, Px] = ekf(state, Pstate, pNoise, oNoise, obs)
%   EKF ----- Main Code
%   Reference: ReBEL, Rudolph van der Merwe
%   (c) Shi Heng (2007)

Xdim  = 6;                                % 状态向量维数
Odim  = 6;                                  % 观测向量维数
Vdim  = 6;                                    % 系统噪声向量维数
Ndim  = 6;                                    % 观测噪声向量维数
NOV = size(obs,2);                                           % 观测向量个数
xh   = zeros(Xdim,NOV);
xh_  = zeros(Xdim,NOV);
zh_  = zeros(Odim,NOV);
inov = zeros(Odim,NOV);

%----------------滤波递推--------------
for i=1:NOV,
    
    % Time Update

        [A] = feval(@linearize, state, pNoise.mu, oNoise.mu,'A');  
        [G] = feval(@linearize, state, pNoise.mu, oNoise.mu,'G');
    xh_(:,i) = feval(@ffun, state, pNoise.mu);
    Px_      = A*Pstate*A' + G*pNoise.cov*G';

    % Measurement Update

        [C] = feval(@linearize, xh_(:,i), pNoise.mu, oNoise.mu,'C');
        [H] = feval(@linearize, state, pNoise.mu, oNoise.mu,'H');
    Py        = C*Px_*C' + H*oNoise.cov*H';
    KG        = Px_ * C' * inv(Py);%%%%%%%%%%%%%%%%%%%%%%%!!!!!!!
    yh_(:,i)  = feval(@hfun, xh_(:,i), oNoise.mu);
        inov(:,i) = obs(:,i) - yh_(:,i);
    xh(:,i)   = xh_(:,i) + KG * inov(:,i);
    Px        = Px_ - KG*Py*KG';

    state  = xh(:,i);
    Pstate = Px;  

end   % loop end