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

netccdn

已于 2023-08-19 08:13:20 修改

阅读量5.9k

点赞数 27

分类专栏：笔记文章标签： matlab 概率论

于 2022-07-04 10:50:33 首次发布

本文链接：https://blog.csdn.net/netccdn/article/details/125586074

版权

笔记专栏收录该内容

18 篇文章

订阅专栏

本文介绍了一种基于变分贝叶斯近似的递归噪声自适应卡尔曼滤波算法，并提供了MATLAB实现代码。通过与传统卡尔曼滤波进行比较，展示了该算法在不确定噪声环境下的鲁棒性和准确性。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

参考文献《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"有程序，效果非常不错。