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

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')









endfunction