组合导航中Kalman滤波算法相关知识简述

温馨提示：阅读本篇博文内容，需要读者具备一定的Kalman滤波基础知识

上图即为Kalman滤波算法的框架，分为预测（时间更新）和更新（量测更新）两部分，其参数估计的过程就是两者循环迭代的过程。

预报，就是根据系统状态方程，从前一时刻状态预测当前时刻的状态的过程，可理解成对系统的先验知识的一种推算。预报中，状态估计和它的方差协方差阵也要给出，从方差协方差阵P的预报公式可以看出，根据系统的推算，推算必然有误差，因此要加一项GQG’。

更新，这里面有一个增益阵的计算，这其实是一个权重的计算，作用是对由预报的状态推算来的观测量和实际观测量的定权。

状态预报的改正，是通过当前状态的改正公式实现的，根据预报状态加上改正项完成。新息向量由外部的观测量z减去观测矩阵和预报的状态向量的乘积。新息向量包含两部分误差，一个是观测误差，一个是预报误差；称为新息向量。是因为该信息是滤波系统新得到的信息，所以称为新息向量。

下面给出利用严老师的PSINS中的代码改的标准卡尔曼滤波来进行演示：

function kf = kfupdate(kf, yk, TimeMeasBoth)
% Discrete-time Kalman filter. 
%
% Prototype: kf = kfupdate(kf, yk, TimeMeasBoth)
% Inputs: kf - Kalman 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 - Kalman filter structure array after time/meas updating
%参数估计的质量往往取决于滤波对于状态异常以及观测异常扰动的认知和控制
%
%
%
% Notes: (1) the Kalman filter stochastic models is
%      xk = Phikk_1*xk_1 + wk_1
%      yk = Hk*xk + vk
%    where E[wk]=0, E[vk]=0, E[wk*wk']=Qk, E[vk*vk']=Rk, E[wk*vk']=0
%    (2) If kf.adaptive=1, then use Sage-Husa adaptive method (but only for 
%    measurement noise 'Rk'). The 'Rk' adaptive formula is:
%      Rk = b*Rk_1 + (1-b)*(rk*rk'-Hk*Pxkk_1*Hk')
%    where minimum constrain 'Rmin' and maximum constrain 'Rmax' are
%    considered to avoid divergence.
%    (3) If kf.fading>1, then use fading memory filtering method.
%    (4) Using Pmax&Pmin to constrain Pxk, such that Pmin<=diag(Pxk)<=Pmax.
%
% See also  kfinit, kfupdatesq, kffk, kfhk, kfc2d, kffeedback, kfplot, RLS, ekf, ukf.

% Copyright(c) 2009-2015, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 08/12/2012, 29/08/2013, 16/04/2015, 01/06/2017, 11/03/2018
    if nargin==1;
        TimeMeasBoth = 'T';
    elseif nargin==2
        TimeMeasBoth = 'B';
    end
    
    if TimeMeasBoth=='T'            % Time Updating
        kf.xk = kf.Phikk_1*kf.xk;    
        kf.Pxk =kf.lambdak*kf.Phikk_1*kf.Pxk*kf.Phikk_1' +kf.Gammak*kf.Qk*kf.Gammak';
    else
        if TimeMeasBoth=='M'        % Meas Updating
            kf.xkk_1 = kf.xk;    
            kf.Pxkk_1 = kf.Pxk; 
        elseif TimeMeasBoth=='B'    % Time & Meas Updating
            kf.xkk_1 = kf.Phikk_1*kf.xk;    
            kf.Pxkk_1 = kf.lambdak*kf.Phikk_1*kf.Pxk*kf.Phikk_1' + kf.Gammak*kf.Qk*kf.Gammak';
        else
            error('TimeMeasBoth input error!');
        end
        %% 以下是求HPH'
        kf.Pxykk_1 = kf.Pxkk_1*kf.Hk';
        kf.Py0 = kf.Hk*kf.Pxykk_1;
        %% 以下是求新息向量
        kf.ykk_1 = kf.Hk*kf.xkk_1;
        kf.rk = yk-kf.ykk_1; %即新息向量
        kf.rki(:,kf.mount)=kf.rk;%存储第i个新息向量，以便后续遗忘滤波算法使用
        
        %% 标准Kalman滤波
        if kf.adaptive==0  
           
            kf.Pykk_1 = kf.Py0 + kf.Rk;
            kf.Kk = kf.Pxykk_1*invbc(kf.Pykk_1); 
            kf.xk = kf.xkk_1 + kf.Kk*kf.rk;
%             kf.Pxk = kf.Pxkk_1 - kf.Kk*kf.Pykk_1*kf.Kk';
            kf.Pxk =(kf.I- kf.Kk*kf.Hk)*kf.Pxkk_1;
            
        end