Kalman filter encapsulation function

background

I need to write several simulations recently , Local standards are heavily used Kalman wave filtering , So I'll just Kalman The filtering algorithm is encapsulated as a function , The subsequent use can be called directly .
Be careful ： The encapsulated function is only the calculation of a single moment , The call needs to be made in the loop body .

function code

%Project:  Local filter function （ Whether the input quantity is universal ）
%Author: Jace
%Data: 2021/10/25
%==================== The body of the function ====================
function [P,xkf,K]=Lkf(Dim,A,H,Gamma,Q,R,Pp,xkfp,z,B,up)
    if nargin==9 % Parameter number judgment 
        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(' Insufficient parameters ');
    end
end

Calling method

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

Corresponding

[ Estimate the covariance at the current time P,  Current time estimate xkf,  Current time gain matrix K]=Lkf( The state dimension ,  State transition matrix A,  Measurement matrix H,  State noise matrix , Gamma（ If not, you can write directly 1）,  State noise covariance Q,  Measurement noise covariance R,  Estimated covariance at the last moment Pp,  Last time estimate xkfp,  Measured value at current time z,  Control matrix B,  Control quantity at the last moment up)

Be careful ： Returns... In the result K And... In the input parameters B、up All are optional parameters , In actual use, you can omit .

Call the test function

%Project:  Basic 2D Kalman Test functions 
%Author: Jace
%Data: 2021/11/02
%==================== Get ready ====================
close all;
clear all;
clc;
%==================== Set global parameters ====================
%-------- Global parameter --------
N=100;% Set the number of sampling points , That is, the duration 
%-------- Set dimension --------
Dim_n=2;% The state dimension 
Dim_m=2;% Measurement dimension 
%-------- System model parameters --------
A=[1.002,0;0,0.998];% State transition matrix 
H=[1,0;0,1];% Measurement matrix 
Gamma=1;

%-------- Noise related parameters --------
P0=0.01;% Initial state noise covariance 

Q=0.01*eye(Dim_n);% Set the system noise 
R=0.1*eye(Dim_m);% Set observation noise 
w=sqrt(Q)*randn(Dim_n,N);
v=sqrt(R)*randn(Dim_m,N);

%==================== Allocate space ========================
%-------- system parameter --------
x=zeros(Dim_n,N);
z=zeros(Dim_m,N);% Measured value 

%--------Kalman Process parameters --------
p=zeros(Dim_n,Dim_n,N);
xkf=zeros(Dim_n,N);% Estimated state 

%==================== initialization ====================
%-------- System parameter initialization --------
x(:,1)=[10+P0*randn(1);20+P0*randn(1)];
z(:,1)=H*x(:,1)+v(:,1);

%-------- Estimate parameter initialization --------
p(:,:,1)=0.1*eye(Dim_n);
xkf(:,1)=x(:,1);

%-------- Specific matrix initialization --------
In=eye(Dim_n);

%==================== iterative process ====================
for k=2:N
    x(:,k)=A*x(:,k-1)+w(:,k);
    z(:,k)=H*x(:,k)+v(:,k);
    %==================== standard Kalman The process ====================
    [p(:,:,k),xkf(:,k)]=Lkf(2,A,H,Gamma,Q,R,p(:,:,k-1),xkf(:,k-1),z(:,k));
end
%====================MSE Calculation ====================
Step=10;
[MSE]=MSE(Dim_n,Step,N,xkf,x);
%==================== mapping ====================
% Local filter 1
figure;
hold on,box on;
plot(x(1,:),'-k.');
plot(xkf(1,:),'-r.');
legend(' The first 1 The status value ',' The first 1 State quantity Kalman Estimated value ');
xlabel(' Sampling time ');ylabel(' Location ');
title(' state 1 track ');

% Local filter 2
figure;
hold on,box on;
plot(x(2,:),'-k.');
plot(xkf(2,:),'-r.');
legend(' The first 2 The status value ',' The first 2 State quantity Kalman Estimated value ');
xlabel(' Sampling time ');ylabel(' Location ');
title(' state 2 track ');