From 60d83a8e639866300e093d69eb24f8d8fe79f2fd Mon Sep 17 00:00:00 2001 From: Jens Ahrensfeld Date: Sun, 31 Mar 2019 11:50:16 +0000 Subject: [PATCH] - added constant acceleration model git-svn-id: http://moon:8086/svn/matlab/trunk@122 801c6759-fa7c-4059-a304-17956f83a07c --- kalman1d_eval.m | 40 +++++++++++++++++++++------------------- 1 file changed, 21 insertions(+), 19 deletions(-) diff --git a/kalman1d_eval.m b/kalman1d_eval.m index f5ab707..2028f9f 100644 --- a/kalman1d_eval.m +++ b/kalman1d_eval.m @@ -59,36 +59,38 @@ if 0 plot(1:N, v_x_, '-r', 1:N, w_, '-b'); grid; legend('v_x', 'w') else - % Model: + % Model: Constant acceleration % x(t) = x(0) + x'(t)*t + 1/2*x''(t)*t² u = [0] % INPUT: Control variable dt = 1 - x = 1 - xs = 0 - P = [[0 0]; % Process covariance matrix P - [0 1]]; + P = [[0 0 0]; % Process covariance matrix P + [0 1 0] + [0 0 1]]; - X = [x xs]'; % State matrix X + X = [0 0 0]'; % State matrix X - A = [[1 dt]; % 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]]; - Bu = [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. + 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]'; % Matrix Z is the measurement noise - Y = [1 0.0]'; % Matrix Y contains measurement data + 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.1*eye(length(P)); % The measurement covariance matrix (R) is the assumed error of the measurement + R = 0.5*eye(length(P)); % The measurement covariance matrix (R) is the assumed error of the measurement _x = []; _y = []; y = 1 ys = 0 - variance = 0.2; + yss = 0 + variance = 1.0; dy = 0.1 + ddy = 0.02 for i=1:100 % PREDICTION % New prediction state X @@ -101,15 +103,15 @@ else % Observation state % ----------------------- - %y = sin(2*pi*i/200) - %ys = cos(2*pi*i/200) - dy = dy + 0.00; +% 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]' + Z; + Y = C*[y ys yss]' + Z; % Update process matrix P and state matrix X I = eye(length(K)); @@ -123,9 +125,9 @@ else end t = 1:length(_x); subplot(2,1,1) - plot(t, _x(1,:), t, _x(2,:)); grid; + plot(t, _x(1,:), t, _y(1,:)); legend('x', 'y'); grid; subplot(2,1,2) - plot(t, _y(1,:), t, _y(2,:)); grid; + plot(t, _x(2,:), t, _y(2,:)); legend('dx', 'dy'); grid; end