## 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 kalman1d_eval()

if 0
	var_meas = 1.0;
	pos_true = 10.0;
	k_motion = 0.0;
	f_motion = 2.0;

	x_h = 0.0;		% initial guess
	v_x = 0.5;		% initial guess
	v_y = 1.0;		% initial guess
	v_y_h_0 = 0;
	v_y_h = 0;

	N = 10000;
	for n=1:N,
		x_h_(n) = x_h;
		v_x_(n) = v_x;
		
		y = pos_true + var_meas*randn() + k_motion*sin(f_motion*2*pi*n/N);
	  w = v_x/(v_x + v_y);
		x_h = x_h + w * (y - x_h);
		v_x = 1/(1/v_x + 1/v_y);
		v_y_h_0 = 0.5*v_y_h_0 + 0.5*(y-x_h)^2;
		y_(n) = y;
		v_y_h = 0.995*v_y_h + 0.005*v_y_h_0;
		v_y_h_(n) = sqrt(v_y_h);
		w_(n) = w;

	end

	subplot(2,1,1)
	plot(1:N, y_, 1:N, x_h_, '-r'); grid; legend('y', 'x_h')
	subplot(2,1,2)
	plot(1:N, v_x_, '-r', 1:N, w_, '-b'); grid; legend('v_x', 'w')

else
	% Model: Constant acceleration
	% x(t) = x(0) + x'(t)*t + 1/2*x''(t)*t²
	u = [0]				% INPUT: Control variable
	dt = 1
	
	P = [[0 0 0];					% Process covariance matrix P
		 [0 1 0]
		 [0 0 1]];

	X = [0 0 0]';				% State matrix X
	
	A = [[1 dt  1/2*dt^2];				% 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. 
		 [0  1  dt ]
		 [0  0  1 ]];
	Bu = [0 1/2*dt^2 dt]' * u;	% 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.
	Q = 0;						% Error term
	w = 0;						% Error term
	Z = [0 0 0]';					% Matrix Z is the measurement noise
	Y = [1 0 0]';				% Matrix Y contains measurement data
	H = eye(length(P));			% Matrix H helps transform the matrix format of P
	C = eye(length(Y));			% Matrix C is a matrix transform to allow it to be summed with Z
	R = 0.5*eye(length(P));		% The measurement covariance matrix (R) is the assumed error of the measurement
	
	_x = [];
	_y = [];
	
	y = 1
	ys = 0
	yss = 0
	variance = 1.0;
	dy = 0.1
	ddy = 0.02
	for i=1:100
		% PREDICTION	
		% New prediction state X
		Xp = A*X + Bu + w;
		Pp = A*P * A' + Q;
		
		% CORRECTION
		% Kalman gain
		K = Pp*H' * inv(H*Pp*H' + R);
		
		% Observation state
		% -----------------------
%		y = sin(2*pi*i/500)
%		ys = cos(2*pi*i/500)
		dy = dy + ddy;
		y = y + dy;
		ys = (y - Y(1))/dt;
		% -----------------------

		Z = variance*randn(size(Z))/sqrt(12);
		Y = C*[y ys yss]'  + Z;
		
		% Update process matrix P and state matrix X
		I = eye(length(K));
		P = (I - K*H)*Pp;
		X = Xp + K*(Y - H*Xp);
		
		% Plot vars
		_x = [_x X];
		_y = [_y Y];

	end
	t = 1:length(_x);
	subplot(2,1,1)
	plot(t, _x(1,:), t, _y(1,:)); legend('x', 'y'); grid;
	subplot(2,1,2)
	plot(t, _x(2,:), t, _y(2,:)); legend('dx', 'dy'); grid;
	
end







endfunction