[Kalman_eval]

- added real sensor data
- added  vargin for parameter

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

kalman_eval.m

@@ -22,20 +22,34 @@
 ## Author: Jens Ahrensfeld <ahrensfeld@w2ess001vm>
 ## Created: 2018-09-28
 
-function kalman_eval()
+function kalman_eval(varargin)
+
+	params = struct ('dt', 1.0, 'var_P', 1.0, 'var_Q', 0, 'var_R', 1.0, 'var_Z', 1.0, 'USE_REAL_DATA', 0);
+
+	% Parse parameters
+	names = fieldnames(params);
+	varargin
+	for k=1:nargin-1,
+		for n=1:length(names),
+			if strcmpi(varargin{k}, names{n})
+				params.(names{n}) = varargin{k+1};
+			end
+		end
+	end
+		
+	print_parameters(params)
 
 	% Model: Constant acceleration
 	% x(t) = x(0) + x'(t)*t + 1/2*x''(t)*t²
-	dt = 1
+	dt = params.dt;
 	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);
+	var_Q = params.var_Q;
+	var_P = params.var_P;
+	var_R = params.var_R;
+	var_Z = params.var_Z;
 	X = zeros(N,1);					% State matrix X
-	P = var_P*eye(N)				% Process covariance matrix P
+	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
 	
@@ -43,22 +57,33 @@ function kalman_eval()
 	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 
+	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 G 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_Q;			% 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
+
+	X = [0 1.0]';
+	xp = [0 -1.0]';
+	num_data = 100;
+
+	if params.USE_REAL_DATA
+		% Data samplerate is 10 sps
+		data = load('temp.log');
+		x = data(:,2);
+		% Downsample to sample rate of 1 sps
+		x = x(1:10:length(x));
+		xp = [x(1) 0.0]';
+		num_data = length(x)
+	end
+	for n=1:num_data
 
 		% State estimate	
 		xp = A*xp;
@@ -69,8 +94,11 @@ function kalman_eval()
 		X = A*X;
 		
 		% Take noisy measurement
-		z = H*X + var_Z.*randn(M,1)/sqrt(12);
-
+		if params.USE_REAL_DATA
+			z = [x(n) 0]';
+		else
+			z = H*X + var_Z*randn(M,1)/sqrt(12);
+		end
 		% ----------------------------
 		
 		% Measurement prediction
@@ -95,21 +123,36 @@ function kalman_eval()
 		P = P - K*S*K';
 
 		% Plot vars
-		_x = [_x X];
+		_x = [_x z];
 		_y = [_y xp];
 
 end
 X
 xp
 
-t = 1:length(_x);
+t = (1:length(_x)) /60*dt;
 subplot(2,1,1)
-plot(t, _x(1,:), t, _y(1,:)); legend('x', 'x_p'); grid;
+plot(t, _x(1,:), t, _y(1,:), '-rx'); legend('x', 'x_p'); grid;
 subplot(2,1,2)
-plot(t, _x(2,:), t, _y(2,:)); legend('xd', 'xd_p'); grid;
-	
+plot(t, _x(2,:), t, _y(2,:)*dt*60, '-rx'); legend('xd', 'xd_p'); grid;
 	
 
+function print_parameters(params)
+align = 32;
+names = fieldnames(params);
+for n=1:length(names),
+	fprintf('%s', names{n});
+	remain = align - length(names{n});
+	if remain < 0
+		error('Invalid variable length!');
+	else
+		for j=1:remain
+			fprintf(' ');
+		end
+		fprintf('= %g\n', params.(names{n}));
+	end
+end
+endfunction
 
 endfunction