PSINS工具箱卡尔曼滤波器

psins中一个15维状态量、3维位置观测量的卡尔曼滤波器示例：

% SINS/GPS intergrated navigation simulation using kalman filter.
% Please run 'test_SINS_trj.m' to generate 'trj10ms.mat' beforehand!!!
% See also  test_SINS_trj, test_SINS, test_SINS_GPS_186, test_SINS_GPS_193.
% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 17/06/2011
glvs
psinstypedef(153); %设定滤波器标识
trj = trjfile('trj10ms.mat');
% initial settings
[nn, ts, nts] = nnts(2, trj.ts);
imuerr = imuerrset(0.03, 100, 0.001, 5); % IMU误差设定
imu = imuadderr(trj.imu, imuerr); % 添加IMU误差
davp0 = avperrset([0.5;-0.5;20], 0.1, [1;1;3]); % 设定初始avp误差
ins = insinit(avpadderr(trj.avp0,davp0), ts); % 初始化INS
% KF filter
rk = poserrset([1;1;3]); % 设定Rk，位置观测误差
kf = kfinit(ins, davp0, imuerr, rk); % 初始化卡尔曼滤波器
kf.Pmin = [avperrset(0.01,1e-4,0.1); gabias(1e-3, [1,10])].^2;  kf.pconstrain=1; % 设定P阵最小值
len = length(imu); [avp, xkpk] = prealloc(fix(len/nn), 10, 2*kf.n+1);
timebar(nn, len, '15-state SINS/GPS Simulation.'); 
ki = 1;
for k=1:nn:len-nn+1
    k1 = k+nn-1;  
    wvm = imu(k:k1,1:6);  t = imu(k1,end);
    ins = insupdate(ins, wvm); % INS更新
    kf.Phikk_1 = kffk(ins); % Phi阵更新
    kf = kfupdate(kf); % 卡尔曼时间更新
    if mod(t,1)==0
        posGPS = trj.avp(k1,7:9)' + davp0(7:9).*randn(3,1);  % GPS pos simulation with some white noise
        kf = kfupdate(kf, ins.pos-posGPS, 'M'); % 卡尔曼观测更新
        [kf, ins] = kffeedback(kf, ins, 1, 'avp'); % 状态反馈
        avp(ki,:) = [ins.avp', t];
        xkpk(ki,:) = [kf.xk; diag(kf.Pxk); t]';  ki = ki+1; % 构建状态量和状态方差数据
    end
    timebar;
end
avp(ki:end,:) = [];  xkpk(ki:end,:) = []; 
% show results
insplot(avp);
avperr = avpcmpplot(trj.avp, avp);
kfplot(xkpk, avperr, imuerr);

psinstypedef

该函数是一个标识器，主要用于设定全局结构体psinsdef，通过psinsdef中的kfinit、kffk、kfhk、kfplot四个字段来标记卡尔曼滤波模型和绘图参数。如153表示采用15维状态向量+3维位置观测模型，156表示采用18维状态向量+3维位置观测+3维速度观测模型。也可以对滤波模型进行修改和增加（需同时修改kfinit, kffk, kfhk, kfplot等函数）。

function psinstypedef(typestr, kffk, kfhk, kfplot)
    ......
    psinsdef.kfinit = typestr;
    psinsdef.kffk = fix(psinsdef.kfinit/10);
    psinsdef.kfhk = psinsdef.kfinit;
    psinsdef.kfplot = psinsdef.kffk;
    ......

kfinit

根据psinsdef中设定的类型，对卡尔曼滤波器进行初始化。主要流程为先根据滤波器模型设定Q阵、R阵、初始P阵、H阵，然后再调用kfinit0函数完成全部初始化工作，返回卡尔曼滤波器结构体kf。

function kf = kfinit(ins, varargin)
switch(psinsdef.kfinit)
    case psinsdef.kfinit153,
        psinsdef.kffk = 15;  psinsdef.kfhk = 153;  psinsdef.kfplot = 15;
        [davp, imuerr, rk] = setvals(varargin);
        kf.Qt = diag([imuerr.web; imuerr.wdb; zeros(9,1)])^2; % 连续域过程噪声矩阵
        kf.Rk = diag(rk)^2; % 离散域观测噪声矩阵
        kf.Pxk = diag([davp; imuerr.eb; imuerr.db]*1.0)^2; % 初始P阵
        kf.Hk = kfhk(0); % 观测矩阵
......
kf = kfinit0(kf, nts); 

kfinit0

卡尔曼滤波器结构体初始化函数，为各状态量、矩阵分配内存空间，包含Q阵、G阵、P阵、K阵、状态量、状态反馈量等（这里的Q、G为离散域矩阵）。同时设置滤波器配置参数，如观测量掩码、自适应滤波开关等。

function kf = kfinit0(kf, nts)
% Always called by kfinit and initialize the remaining fields of kf.
%
% See also kfinit, kfupdate, kffeedback, psinstypedef.
    kf.nts = nts;
    [kf.m, kf.n] = size(kf.Hk); % m,观测维数；n,状态维数
    kf.I = eye(kf.n);
    kf.Kk = zeros(kf.n, kf.m); % 卡尔曼增益矩阵
    kf.measmask = [];            % measurement mask for no update  20/11/2022
    kf.measstop = zeros(kf.m,1); % measurement stop time
    kf.measlost = zeros(kf.m,1); % measurement lost time
    kf.measlog = 0;              % measurement log flag
    if ~isfield(kf, 'xk'),  kf.xk = zeros(kf.n, 1);  end % 状态向量
    if ~isfield(kf, 'Qk'),  kf.Qk = kf.Qt*kf.nts;  end % 过程噪声矩阵
    if ~isfield(kf, 'Gammak'),  kf.Gammak = 1; kf.l = kf.n;  end % 噪声传递矩阵
    if ~isfield(kf, 'fading'),  kf.fading = 1;  end
    if ~isfield(kf, 'adaptive'),  kf.adaptive = 0;  end
    ......
    kf.Pmax = (diag(kf.Pxk)+1)*1.0e10;
    kf.Pmin = kf.Pmax*0;
%     kf.Pykk_1 = kf.Hk*kf.Pxk*kf.Hk'+kf.Rk;
    kf.Pykk_1 = kf.Hk*kf.Pxk*kf.Hk'+0; % 计算卡尔曼增益的中间矩阵
    kf.xfb = zeros(kf.n, 1); % 状态反馈量
%     kf.coef_fb = (1.0-exp(-kf.T_fb./kf.xtau));
%     kf.coef_fb = ar1coefs(kf.T_fb, kf.xtau);
    xtau = kf.xtau;
    xtau(kf.xtau<kf.T_fb) = kf.T_fb;  kf.coef_fb = kf.T_fb./xtau;

kffk

根据滤波器模型调用etm函数更新F阵，并离散化为Phi阵。etm函数计算F阵的元素值。

function [Fk, Ft] = kffk(ins, varargin)
global psinsdef
    %% get Ft
    if isstruct(ins),    nts = ins.nts;
    else                 nts = ins;
    end
    switch(psinsdef.kffk)
        case 15
            Ft = etm(ins);
        ......
    end
    %% discretization，离散化
	Fk = Ft*nts;
    if nts>0.1  % for large time interval, this may be more accurate.
        Fk = expm(Fk);
    else   % Fk = I + Ft*nts + 1/2*(Ft*nts)^2  , 2nd order expension
        Fk = eye(size(Ft)) + Fk;% + Fk*Fk*0.5; 
    end

kfhk

设置观测矩阵H。

function Hk = kfhk(ins, varargin)
global psinsdef
    switch(psinsdef.kfhk)
        case 153,
            Hk = [zeros(3,6), eye(3), zeros(3,6)];
    end
    ......

kfupdate

卡尔曼更新函数。输入参数kf结构体、观测量yk（可选参数，执行观测更新时需传入）、更新标识TimeMeasBoth。TimeMeasBoth为T时只进行时间更新，为M时只进行观测更新，为B时执行完整的时间更新和观测更新。

function kf = kfupdate(kf, yk, TimeMeasBoth)
    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.Phikk_1*kf.Pxk*kf.Phikk_1' + kf.Gammak*kf.Qk*kf.Gammak';
        kf.measstop = kf.measstop - kf.nts;  kf.measlost = kf.measlost + kf.nts;
    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.Phikk_1*kf.Pxk*kf.Phikk_1' + kf.Gammak*kf.Qk*kf.Gammak';
            kf.measstop = kf.measstop - kf.nts;  kf.measlost = kf.measlost + kf.nts;
        else
            error('TimeMeasBoth input error!');
        end
    ......
    end

kffeedback

状态反馈，psins中的状态量均为误差量。执行卡尔曼滤波后，可将状态量通过kffeedback函数反馈值全状态。

function [kf, ins, xfb] = kffeedback(kf, ins, T_fb, fbstr)
    if nargin<4, fbstr=kf.fbstr; end
    if nargin<3, T_fb=1; end
    if kf.T_fb~=T_fb
        kf.T_fb = T_fb;
        idx = kf.T_fb>kf.xtau;  % scale<vector
        kf.coef_fb(idx) = 1;  kf.coef_fb(~idx) = kf.T_fb./kf.xtau(~idx);   %2022-6-25
        kf.coef_fb(kf.xtau<0.001 & kf.T_fb~=inf) = 1;
    end
    xfb_tmp = kf.coef_fb.*kf.xk;  xfb = xfb_tmp*0;
    for k=1:length(fbstr)
        switch fbstr(k)
            case 'a',
                idx = 1:3; ins.qnb = qdelphi(ins.qnb, xfb_tmp(idx));
            case 'v',
                idx = 4:6; ins.vn = ins.vn - xfb_tmp(idx);
            case 'p',
                idx = 7:9; ins.pos = ins.pos - xfb_tmp(idx);
            ......
        end
        kf.xk(idx) = kf.xk(idx) - xfb_tmp(idx);    %
        kf.xfb(idx) = kf.xfb(idx) + xfb_tmp(idx);  % record total feedback
        xfb(idx) = xfb_tmp(idx);
    end
    [ins.qnb, ins.att, ins.Cnb] = attsyn(ins.qnb);
    ins.avp = [ins.att; ins.vn; ins.pos];  % 2015-2-22