- added constant acceleration model

git-svn-id: http://moon:8086/svn/matlab/trunk@122 801c6759-fa7c-4059-a304-17956f83a07c

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
+	P = [[0 0 0];					% Process covariance matrix P
-		 [0 1]];
+		 [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. 
+	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]];
+		 [0  1  dt ]
-	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.
+		 [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
+	Z = [0 0 0]';					% Matrix Z is the measurement noise
-	Y = [1 0.0]';				% Matrix Y contains measurement data
+	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)
+%		y = sin(2*pi*i/500)
-		%ys = cos(2*pi*i/200)
+%		ys = cos(2*pi*i/500)
-		dy = dy + 0.00;
+		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