Kalman滤波封装Matlab函数

背景

本人最近需要写多个仿真，需要大量用到本地标准Kalman滤波，于是干脆将Kalman滤波的算法封装为函数，后续使用直接进行调用即可。
注意：封装的函数仅仅是单一时刻的计算，调用需要在循环体内进行。

函数代码

%Project: 本地滤波器函数（有无输入量通用）
%Author: Jace
%Data: 2021/10/25
%====================函数体====================
function [P,xkf,K]=Lkf(Dim,A,H,Gamma,Q,R,Pp,xkfp,z,B,up)
    if nargin==9 %参数个数判断
        I=eye(Dim);
        P_pre=A*Pp*A'+Gamma*Q*Gamma';       
        K=P_pre*H'/(H*P_pre*H'+R);  
        xkf=A*xkfp+K*(z-H*A*xkfp);
        P=(I-K*H)*P_pre; 
    elseif nargin==11
        I=eye(Dim);
        P_pre=A*Pp*A'+Gamma*Q*Gamma';       
        K=P_pre*H'/(H*P_pre*H'+R);  
        x_pre=A*xkfp+B*up;
        xkf=x_pre+K*(z-H*x_pre);
        P=(I-K*H)*P_pre; 
    else
        error('参数不足');
    end
end

调用方法

[P,xkf,K]=Lkf(Dim,A,H,Gamma,Q,R,Pp,xkfp,z,B,up)

对应

[当前时刻估计协方差P, 当前时刻估计值xkf, 当前时刻增益矩阵K]=Lkf(状态维度, 状态转移矩阵A, 量测矩阵H, 状态噪声矩阵, Gamma（如果没有可以直接写1）, 状态噪声协方差Q, 量测噪声协方差R, 上一时刻估计协方差Pp, 上一时刻估计值xkfp, 当前时刻量测值z, 控制矩阵B, 上一时刻控制量up)

注意：返回结果中的K和输入参数中的B、up均为可选参数，实际使用时可以省略。

调用测试函数

%Project: 基本二维Kalman测试本地Kalman滤波函数
%Author: Jace
%Data: 2021/10/06
%====================准备====================
close all;
clear all;
clc;
%====================设定全局参数====================
%--------全局参数--------
N=100;%设定采样点数，即持续时长
%--------设定维度--------
Dim_n=2;%状态维度
Dim_m=2;%量测维度
%--------系统模型参数--------
A=[1,0.1;0,1];%状态转移矩阵
H=[1,0;0,1];%局部量测1量测矩阵
Gamma=1;

%--------噪声相关参数--------
P0=0.01;%初始状态噪声协方差

Q=0.01*eye(Dim_n);%设定系统噪声
R=0.1*eye(Dim_m);%设定观测噪声
w=sqrt(Q)*randn(Dim_n,N);
v=sqrt(R)*randn(Dim_m,N);

%====================分配空间========================
%--------系统参数--------
x=zeros(Dim_n,N);
z=zeros(Dim_m,N);%量测值

%--------Kalman过程参数--------
p=zeros(Dim_n,Dim_n,N);
xkf=zeros(Dim_n,N);%估计状态

%====================初始化====================
%--------系统参数初始化--------
x(:,1)=[10+P0*randn(1);20+P0*randn(1)];%物体初始真实状态值
z(:,1)=H*x(:,1)+v(:,1);%观测真实值初始值

%--------估计参数初始化--------
p(:,:,1)=0.1*eye(Dim_n);%误差协方差初始值
xkf(:,1)=x(:,1);%全局估计状态初始化

%--------特定矩阵初始化--------
In=eye(Dim_n);%2*2单位矩阵

%====================迭代过程====================
for k=2:N
    
    %系统模型
    x(:,k)=A*x(:,k-1)+w(:,k);
    %量测模型，标量
    z(:,k)=H*x(:,k)+v(:,k);
    %====================标准Kalman过程====================
    [p(:,:,k),xkf(:,k)]=Lkf(2,A,H,Gamma,Q,R,p(:,:,k-1),xkf(:,k-1),z(:,k));
end
%====================MSE计算====================
Step=10;
[MSE]=MSE(Dim_n,Step,N,xkf,x);
%====================绘图====================
%局部滤波器1
figure;
hold on,box on;
plot(x(1,:),'-k.');
plot(xkf(1,:),'-r.');
legend('第1状态值','第1状态量Kalman估计值');
xlabel('采样时间');ylabel('位置');

%局部滤波器2
figure;
hold on,box on;
plot(x(2,:),'-k.');
plot(xkf(2,:),'-r.');
legend('第2状态值','第2状态量Kalman估计值');
xlabel('采样时间');ylabel('位置');

%MSE
figure;
hold on,box on;
plot(MSE(1,:),'-r.');
plot(MSE(2,:),'-g.');
legend('MSE1','MSE2');
xlabel('采样时间');ylabel('数值');

仿真效果