jens
f59fb8f67d
git-svn-id: http://moon:8086/svn/matlab/trunk@100 801c6759-fa7c-4059-a304-17956f83a07c
70 lines
1.6 KiB
Objective-C
70 lines
1.6 KiB
Objective-C
## 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
|
|
|