kalmanf.m

来自「时域卡尔曼滤波器,自己谨慎编写,学习滤波器必备!」· M 代码 · 共 73 行

clear s
s.x = 12;
s.A = 1;
% % Define a process noise (stdev) of 2 volts as the car operates:
s.Q = 2^2; % variance, hence stdev^2
% Define the voltimeter to measure the voltage itself:
s.H = 1;
% % Define a measurement error (stdev) of 2 volts:
s.R = 2^2; % variance, hence stdev^2
%Do not define any system input (control) functions:
s.B = 0;
s.u = 0;
% % Do not specify an initial state:
s.x = nan;
s.P = nan;
% % Generate random voltages and watch the filter operate.
tru=[]; % truth voltage
for t=1:20
tru(end+1) = randn*2+12;
s(end).z = tru(end) + randn*2; % create a measurement
s(end+1)=kalmanf(s(end)); % perform a Kalman filter iteration
% end
% figure
% hold on
% grid on
% % plot measurement data:
hz=plot([s(1:end-1).z],'r.');
% % plot a-posteriori state estimates:
hk=plot([s(2:end).x],'b-');
ht=plot(tru,'g-');
legend([hz hk ht],'observations','Kalman output','true voltage',0)
title('Automobile Voltimeter Example')
% hold off

function s = kalmanf(s)

% set defaults for absent fields:
if ~isfield(s,'x'); s.x=nan*z; end
if ~isfield(s,'P'); s.P=nan; end
if ~isfield(s,'z'); error('Observation vector missing'); end
if ~isfield(s,'u'); s.u=0; end
if ~isfield(s,'A'); s.A=eye(length(x)); end
if ~isfield(s,'B'); s.B=0; end
if ~isfield(s,'Q'); s.Q=zeros(length(x)); end
if ~isfield(s,'R'); error('Observation covariance missing'); end
if ~isfield(s,'H'); s.H=eye(length(x)); end

if isnan(s.x)
% initialize state estimate from first observation
if diff(size(s.H))
error('Observation matrix must be square and invertible for state autointialization.');
end
s.x = inv(s.H)*s.z;
s.P = inv(s.H)*s.R*inv(s.H'); 
else

% This is the code which implements the discrete Kalman filter:

% Prediction for state vector and covariance:
s.x = s.A*s.x + s.B*s.u;
s.P = s.A * s.P * s.A' + s.Q;

% Compute Kalman gain factor:
K = s.P*s.H'*inv(s.H*s.P*s.H'+s.R);

% Correction based on observation:
s.x = s.x + K*(s.z-s.H*s.x);
s.P = s.P - K*s.H*s.P;

end

return