- extended to derivative

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

kalman1d_eval.m

@@ -24,41 +24,111 @@
 
 function kalman1d_eval()
 
-var_meas = 1.0;
-pos_true = 10.0;
-k_motion = 0.0;
-f_motion = 2.0;
+if 0
+	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;
+	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;
+	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')
+
+else
+	% Model:
+	% x(t) = x(0) + x'(t)*t + 1/2*x''(t)*t²
+	u = [0]				% INPUT: Control variable
+	dt = 1
+	x = 1
+	xs = 0
 	
-	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;
+	P = [[0 0];					% Process covariance matrix P
+		 [0 1]];
 
+	X = [x xs]';				% 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.
+	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
+	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
+	
+	_x = [];
+	_y = [];
+	
+	y = 1
+	ys = 0
+	variance = 0.2;
+	dy = 0.1
+	for i=1:100
+		% PREDICTION	
+		% New prediction state X
+		Xp = A*X + Bu + w;
+		Pp = A*P * A' + Q;
+		
+		% CORRECTION
+		% Kalman gain
+		K = Pp*H' * inv(H*Pp*H' + R);
+		
+		% Observation state
+		% -----------------------
+		%y = sin(2*pi*i/200)
+		%ys = cos(2*pi*i/200)
+		ys = (Y(1) - y)/dt;
+		y = y + dy;
+		dy = dy + 0.0
+		% -----------------------
+
+		Z = variance*randn(size(Z))/sqrt(12);
+		Y = C*[y ys]' + Z;
+		
+		% Update process matrix P and state matrix X
+		I = eye(length(K));
+		P = (I - K*H)*Pp;
+		X = Xp + K*(Y - H*Xp);
+		
+		% Plot vars
+		_x = [_x X];
+		_y = [_y Y];
+
+	end
+	t = 1:length(_x);
+	subplot(2,1,1)
+	plot(t, _x(1,:), t, _x(2,:)); grid;
+	subplot(2,1,2)
+	plot(t, _y(1,:), t, _y(2,:)); grid;
+	
 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')
-
-
-