基于变分贝叶斯的自适应卡尔曼滤波（matlab）

    参考文献《Recursive Noise Adaptive Kalman Filtering by Variational Bayesian Approximations》中的算法，用matlab编写了一个小程序测试了一下（demo版本）。程序是按照文章中的内容实现的。
    下面是主程序：

clear all;
close all;
%%%%%Model parameters%%%%%%%
nxp = 10;
nx = 4;     % number of state variables
nz = 2;     % number of measures
T = 1;
q = 1;
r = 1;
F = [eye(2) T*eye(2);zeros(2) eye(2)];
H = [eye(2) zeros(2)];
Q0 = q * [T^3/3*eye(2) T^2/2*eye(2);T^2/2*eye(2) T*eye(2)];
R0 = r * [1 0.5;0.5 1];
L = 1000;
Tn = L;
N = 5;                 %%%%%%The number of variational iteration
times_Of_R = 5;
%%%%%Initial values
x_Ini = [100;100;10;10];
P_Ini = diag([100 100 100 100]);
X = zeros(4,L);
XKF_True = zeros(4,L);
XKF_Const = zeros(4,L);
XKF_VB = zeros(4,L);

mse_Kf_1 = zeros(L,nxp);
mse_Kf_2 = zeros(L,nxp);
mse_Ktf_1 = zeros(L,nxp);
mse_Ktf_2 = zeros(L,nxp);
mse_VB_1 = zeros(L,nxp);
mse_VB_2 = zeros(L,nxp);
for k = 1:nxp
    x=x_Ini;
    X(:,1) = x;
    %%%%Kalman filter with nominal noise covariance matrices (KFNCM)
    XKF_Const(:,1) = x;
    x1=x;
    P_Const = P_Ini;
    %%%%Kalman filter with true noise covariance matrices (KFTCM)
    XKF_True(:,1) = x;
    x2 = x;
    P_True = P_Ini;
    
    %%%%Kalman filter of variational Bayesian Approximations
    XKF_VB(:,1) = x;
    x3 = x;
    P_VB = P_Ini;
    alfa = [1 1]';
    beta = diag(R0);
    mk = x;
    for t = 2:Tn
        %%%%True noise covariance matrices
        Q = (1+0.5*cos(pi*t/Tn))*Q0;
        R = (1+0.6*cos(pi*t/Tn))*R0;
        %%%%Square-root of noise covariance matrices
        SQ = utchol(Q);
        SR = utchol(R);
        %%%%Simulate true state and measurement
        x = F*x+SQ*randn(nx,1);
        z = H*x+SR*randn(nz,1);
        X(:,t) = x;
        %%%%Filtering
        [x1,P_Const,Ppf] = kf(x1,P_Const,F,H,z,Q0,R0*times_Of_R);
        [x2,P_True,Pptf] = kf(x2,P_True,F,H,z,Q,R);
        [x3,P_VB,alfa,beta,mk] = vbkf(x3,P_VB,alfa,beta,mk,F,H,z,Q0,N);
        %%%%Save data
        XKF_Const(:,t) = x1;
        XKF_True(:,t) = x2;
        XKF_VB(:,t) = x3;
    end
    %%%%MSE calculation
    mse_Kf_1(:,k) = (X(1,:)-XKF_Const(1,:)).^2+(X(2,:)-XKF_Const(2,:)).^2;
    mse_Kf_2(:,k) = (X(3,:)-XKF_Const(3,:)).^2+(X(4,:)-XKF_Const(4,:)).^2;

    mse_Ktf_1(:,k) = (X(1,:)-XKF_True(1,:)).^2+(X(2,:)-XKF_True(2,:)).^2;
    mse_Ktf_2(:,k) = (X(3,:)-XKF_True(3,:)).^2+(X(4,:)-XKF_True(4,:)).^2;

    mse_VB_1(:,k) = (X(1,:)-XKF_VB(1,:)).^2+(X(2,:)-XKF_VB(2,:)).^2;
    mse_VB_2(:,k) = (X(3,:)-XKF_VB(3,:)).^2+(X(4,:)-XKF_VB(4,:)).^2;
end

%%%%%%%%%RMSE calculation
rmse_Kf_1 = sqrt(mean(mse_Kf_1,2));
rmse_Kf_2 = sqrt(mean(mse_Kf_2,2));
rmse_Ktf_1 = sqrt(mean(mse_Ktf_1,2));
rmse_Ktf_2 = sqrt(mean(mse_Ktf_2,2));

rmse_VB_1 = sqrt(mean(mse_VB_1,2));
rmse_VB_2 = sqrt(mean(mse_VB_2,2));
%%%%%%%RMSE curves
figure;
j = 2:L;
subplot(2,1,1)
plot(j*T,rmse_Kf_1(2:end),'-b',j*T,rmse_Ktf_1(2:end),'-g',j*T,rmse_VB_1(2:end),'--r','linewidth',2);
ylabel('RMSE_{pos} (m)');
subplot(2,1,2)
plot(j*T,rmse_Kf_2(2:end),'-b',j*T,rmse_Ktf_2(2:end),'-g',j*T,rmse_VB_2(2:end),'--r','linewidth',2);
xlabel('Time (s)');
ylabel('RMSE_{vel} (m/s)');
legend('KF1','KF2','The VBKF');

    下面是调用到的几个函数：
    函数一（cholesky分解，引用了其它的程序）：

function C = utchol(P)
%
% 
% M. S. Grewal & A. P. Andrews
% Kalman Filtering Theory and Practice Using MATLAB
% Third Edition, Wiley & Sons, 2008
% 
% for P symmetric and positive definite,
% computes upper triangular C such that
% C*C' = P
%
[n,m] = size(P);
if (n-m) error('non-square argument'); end;
  for j=m:-1:1,
    for i=j:-1:1,
    sigma = P(i,j);
      for k=j+1:m,
      sigma = sigma - C(i,k)*C(j,k);
      end;
    C(j,i) = 0;
      if (i==j)
        C(i,j) = sqrt(max([0,sigma]));
      elseif (C(j,j) == 0)
        C(i,j) = 0;
      else
        C(i,j) = sigma/C(j,j);
      end;
    end;
  end;
      

    函数二（标准卡尔曼滤波）：

% This program impliment the standard kalman filter
function [x,P,P_Pre]=kf(x,P,F,H,z,Q,R)

x_Pre=F*x;
P_Pre=F*P*F'+Q;

S=H*P_Pre*H'+R;
K=P_Pre*H'*pinv(S);
x=x_Pre+K*(z-H*x_Pre);
P=P_Pre-K*S*K';

    函数三：

% This program give an implication of the algorithms in the article
% <Recursive Noise Adaptive Kalman Filtering by Variational Bayesian Approximations>
function [x_VB,P,alfa,beta,mk,R_VB] = vbkf(x_VB,P,alfa,beta,mk,F,H,z,Q0,N)
% x_VB  state variable
% P covariance matrix

rou = 0.99;
alfa = alfa+0.5;
alfa = rou*alfa;
beta = rou*beta;
betaBak = beta;
%%%%%Time update
x_Pre = F*x_VB;
P_pre = F*P*F'+Q0;
mk_Pre = F*mk;

for i = 1:N
    R_VB = diag(beta./alfa);
    mk = mk_Pre+P_pre*H'*pinv(H*P_pre*H'+R_VB)*(z-H*mk_Pre);
    P = P_pre-P_pre*H'*pinv(H*P_pre*H'+R_VB)*H*P_pre;
    beta = betaBak+1/2*(z-H*mk).^2+1/2*diag(H*P*H');
end
K = P_pre*H'*pinv(H*P_pre*H'+R_VB);
x_VB = x_Pre+K*(z-H*x_Pre);

    下面是一个效果图：

    注1：demo版；mathworks同时也上传了一份。
    注2：网址Matlab code for the paper "A Novel Robust Student’s t-Based Kalman Filter"有程序，效果非常不错。