jens
080d53548f
git-svn-id: http://moon:8086/svn/matlab/trunk@123 801c6759-fa7c-4059-a304-17956f83a07c
116 lines
3.6 KiB
Matlab
116 lines
3.6 KiB
Matlab
## 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()
|
||
|
||
% Model: Constant acceleration
|
||
% x(t) = x(0) + x'(t)*t + 1/2*x''(t)*t²
|
||
dt = 1
|
||
M = [1 dt 1/2*dt^2];
|
||
N = length(M)-1;
|
||
var_u = 0.0
|
||
var_a = 0.0
|
||
var_P = 1.0
|
||
var_R = 1.0
|
||
var_Z = 1.0*ones(N,1);
|
||
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
|
||
A = A
|
||
|
||
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 B 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_a % 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;
|
||
X = [0 1.0]';
|
||
xp = [0 -0.5]';
|
||
|
||
% Known are xp (k|k ), u(k ), P(k|k ) and the new measurement z(k+1).
|
||
_x = [];
|
||
_y = [];
|
||
for n=1:100
|
||
|
||
% State estimate
|
||
xp = A*xp;
|
||
|
||
% ----------------------------
|
||
% New measurement
|
||
% Process ground truth
|
||
X = A*X;
|
||
|
||
% Take noisy measurement
|
||
z = H*X + var_Z.*randn(M,1)/sqrt(12);
|
||
|
||
% ----------------------------
|
||
|
||
% 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 X];
|
||
_y = [_y xp];
|
||
|
||
end
|
||
X
|
||
xp
|
||
|
||
t = 1:length(_x);
|
||
subplot(2,1,1)
|
||
plot(t, _x(1,:), t, _y(1,:)); legend('x', 'x_p'); grid;
|
||
subplot(2,1,2)
|
||
plot(t, _x(2,:), t, _y(2,:)); legend('xd', 'xd_p'); grid;
|
||
|
||
|
||
|
||
|
||
endfunction
|
||
|