jens/matlab
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

- added constant acceleration model

Browse Source 
git-svn-id: http://moon:8086/svn/matlab/trunk@122 801c6759-fa7c-4059-a304-17956f83a07c
This commit is contained in:
jens
2019-03-31 11:50:16 +00:00
parent 4f18293ec4
commit 60d83a8e63
1 changed files with 21 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
kalman1d_eval.m
+21 -19
View File
@@ -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