Contents

Extended Kalman filter for a nonlinear spring.

Loads the stored mat-file KFSim. This contains the simulation results generated by NLSpringSim.

Demonstrates an Extended Kalman Filter with a nonlinear dynamical system and nonlinear measurement. The filter tracks the system reasonably well. ------------------------------------------------------------------------ See also: RHSNLSpring, KFInitialize, EKFPredict, EKFUpdate, TimeLabl, Plot2D ------------------------------------------------------------------------

%--------------------------------------------------------------------------
%   Copyright (c) 2020 Princeton Satellite Systems, Inc.
%   All rights reserved.
%--------------------------------------------------------------------------
%   Since 2020.1
%--------------------------------------------------------------------------

Set up the filter

s           = load('KFSim');
n           = s.n; % Number of steps
r           = s.noiseMeas^2; % Measurement noise
q           = s.noiseForce^2; % Plant noise
xE          = [0;0]; % Estimated state
p           = [0 0;0 0]; % Initial covariance
xP          = zeros(7,n);
s.d.w       = s.w;

% Initialize the filter
dEKF        = KFInitialize( 'ekf','f',@RHSNLSpring,'dT',s.dT,...
                            'fData',s.d,'h',@MeasNLSpring,...
                            'hX',@MeasPartialNLSpring,'hData',s.d,...
                            'fX',@RHSPartialNLSpring,'p',p,...
                            'q',q,'x',xE,'m',xE,'r',r);

EKF Estimation Loop

t = 0;
for k = 1:n
  % Measurement
  z = s.xP(3,k);

  % Store variables to plot
  xP(:,k)       = [z;diag(dEKF.p);s.xP(1:2,k);dEKF.m];

	% Extended Kalman Filter
  dEKF.t        = t;
	dEKF.y        = z;
	dEKF          = EKFPredict( dEKF );
	dEKF          = EKFUpdate( dEKF );
  t             = t + s.dT;
end

Plot

yL = {'z' 'p_x' 'p_v' 'x' 'v' };
lL = {{'Truth','Estimate'} {'Truth','Estimate'}};

[t,tL] = TimeLabl(0:(n-1)*s.dT);
Plot2D(t,xP(1:3,:),tL,yL(1:3),'Kalman Filter');
Plot2D(t,xP(4:7,:),tL,yL(4:5),'Kalman Filter',...
  'lin',{'[1 3]' '[2 4]'},[],[],[],[],lL);


%--------------------------------------
% $Date$
% $Id: 4c7d8a27a030563958ba95f987503e5f3e5f8406 $

Published with MATLAB® R2023b