matlab/kalman_eval.m

## Copyright (C) 2018 Jens Ahrensfeld
## 
## This program is free software; you can redistribute it and/or modify it
## under the terms of the GNU General Public License as published by
## the Free Software Foundation; either version 3 of the License, or
## (at your option) any later version.
## 
## This program is distributed in the hope that it will be useful,
## but WITHOUT ANY WARRANTY; without even the implied warranty of
## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
## GNU General Public License for more details.
## 
## You should have received a copy of the GNU General Public License
## along with this program.  If not, see <http://www.gnu.org/licenses/>.

## -*- texinfo -*- 
## @deftypefn {Function File} {@var{retval} =} kalman1d_eval (@var{input1}, @var{input2})
##
## @seealso{}
## @end deftypefn

## Author: Jens Ahrensfeld <ahrensfeld@w2ess001vm>
## Created: 2018-09-28

function kalman_eval(varargin)

	params = struct ('dt', 1.0, 'var_P', 1.0, 'var_Q', 0, 'var_R', 1.0, 'var_Z', 1.0, 'USE_REAL_DATA', 0);

	% Parse parameters
	names = fieldnames(params);
	varargin
	for k=1:nargin-1,
		for n=1:length(names),
			if strcmpi(varargin{k}, names{n})
				params.(names{n}) = varargin{k+1};
			end
		end
	end
		
	print_parameters(params)

	% Model: Constant acceleration
	% x(t) = x(0) + x'(t)*t + 1/2*x''(t)*t²
	dt = params.dt;
	M = [1 dt 1/2*dt^2];
	N = length(M)-1;
	var_Q = params.var_Q;
	var_P = params.var_P;
	var_R = params.var_R;
	var_Z = params.var_Z;
	X = zeros(N,1);					% State matrix X
	P = var_P*eye(N);				% Process covariance matrix P
	R = var_R*eye(N);				% The measurement covariance matrix (R) is the assumed error of the measurement
	H = eye(N);						% Matrix H helps transform the matrix format of P
	
	A = eye(N);						% Matrix A times x represents the current state and velocity based on the next time step (delta t). A time step is taken, and the velocity is added onto the previous position to update the position of the object. The velocity remains the same. The velocity may have changed after the time step due to acceleration (control variable matrix). If there was acceleration, than this calculation isn’t complete since the acceleration would’ve affected the velocity. 
	for row=1:N,
		A(row,row:N) = M(1:N-row+1);
	end
	 
	B = [1/2*dt^2 dt]';			% The B matrix mimics part of the kinematics equation where the velocity and acceleration are multiplied by time. When matrix B is multiplied by the control variable u (in this case, acceleration) and added to AX, it results in a change to the position and velocity due to acceleration.
	G = [1/2*dt^2 dt]';			% The G matrix mimics part of the kinematics equation where the velocity and acceleration are multiplied by time. When matrix G is multiplied by the control variable u (in this case, acceleration) and added to AX, it results in a change to the position and velocity due to acceleration.
	Q = G*G' * var_Q;			% Error term 
	Z = zeros(N,1);				% Matrix Z is the measurement noise
	Y = zeros(N,1);				% Matrix Y contains measurement data
	
	H(2,2) = 0;
	
	% Known are xp (k|k ), u(k ), P(k|k ) and the new measurement z(k+1).
	_x = [];
	_y = [];

	X = [0 1.0]';
	xp = [0 -1.0]';
	num_data = 100;

	if params.USE_REAL_DATA
		% Data samplerate is 10 sps
		data = load('temp.log');
		x = data(:,2);
		% Downsample to sample rate of 1 sps
		x = x(1:10:length(x));
		xp = [x(1) 0.0]';
		num_data = length(x)
	end
	for n=1:num_data

		% State estimate	
		xp = A*xp;
		
		% ----------------------------
		% New measurement
		% Process ground truth
		X = A*X;
		
		% Take noisy measurement
		if params.USE_REAL_DATA
			z = [x(n) 0]';
		else
			z = H*X + var_Z*randn(M,1)/sqrt(12);
		end
		% ----------------------------
		
		% Measurement prediction
		zp = H*xp;
		
		% Measurement residual
		v = z  - zp;

		% State prediction covariance
		P = A*P * A' + Q;

		% Measurement prediction covariance
		S = H*P*H' + R;
		
		% Kalman gain
		K = P*H' * inv(S);
		
		% Update state estimate
		xp = xp + K*v;

		% Updated state covariance
		P = P - K*S*K';

		% Plot vars
		_x = [_x z];
		_y = [_y xp];

end
X
xp

t = (1:length(_x)) /60*dt;
subplot(2,1,1)
plot(t, _x(1,:), t, _y(1,:), '-rx'); legend('x', 'x_p'); grid;
subplot(2,1,2)
plot(t, _x(2,:), t, _y(2,:)*dt*60, '-rx'); legend('xd', 'xd_p'); grid;
	

function print_parameters(params)
align = 32;
names = fieldnames(params);
for n=1:length(names),
	fprintf('%s', names{n});
	remain = align - length(names{n});
	if remain < 0
		error('Invalid variable length!');
	else
		for j=1:remain
			fprintf(' ');
		end
		fprintf('= %g\n', params.(names{n}));
	end
end
endfunction

endfunction