基于变分贝叶斯的自适应卡尔曼滤波（matlab）-Toy模板网

这篇具有很好参考价值的文章主要介绍了基于变分贝叶斯的自适应卡尔曼滤波（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);

下面是一个效果图：
基于变分贝叶斯的自适应卡尔曼滤波（matlab）
注1：demo版；mathworks同时也上传了一份。
注2：网址Matlab code for the paper "A Novel Robust Student’s t-Based Kalman Filter"有程序，效果非常不错。文章来源地址https://www.toymoban.com/news/detail-504805.html