*** empty log message ***

sw/logalizer/matlab/ahrs.m

@@ -117,14 +117,14 @@ quat_out = quat_in + quat_dot * dt;
 
 quat_out = normalize_quat(quat_out);
 
-% A is the Jacobian of Xdot with respect to the states
+% F is the Jacobian of Xdot with respect to the states
 q0 = quat_out(1);
 q1 = quat_out(2);
 q2 = quat_out(3);
 q3 = quat_out(4);
 
 
-A =  0.5 * [ 0 -p -q -r  q1  q2  q3
+F =  0.5 * [ 0 -p -q -r  q1  q2  q3
 	     p  0  r -q -q0  q3 -q2 
 	     q -r  0  p -q3 -q0  q1
 	     r  q -p  0  q2 -q1 -q0 
@@ -132,7 +132,7 @@ A =  0.5 * [ 0 -p -q -r  q1  q2  q3
 	     0  0  0  0  0   0   0 
 	     0  0  0  0  0   0   0 ];
 
-P_dot = A * P_in + P_in * A' + Q_in;
+P_dot = F * P_in + P_in * F' + Q_in;
 
 P_out = P_in + P_dot * dt;
 

sw/logalizer/matlab/run_ahrs_on_log.m

@@ -1,10 +1,8 @@
 clear
 
-log = 'data/log_ahrs_bug';
-%log = 'data/log_imu_still';
-%log = 'data/log_imu_roll';
+%log = 'data/log_ahrs_bug';
 %log = 'data/log_ahrs_still';
-%log = 'data/log_ahrs_roll';
+log = 'data/log_ahrs_roll';
 %log = 'data/log_ahrs_yaw';
 %log = 'data/log_ahrs_yaw_pitched';
 

sw/logalizer/matlab/run_tilt.m

@@ -2,23 +2,24 @@ clear
 
 dt = 0.015625;
 
-use_log = 0;
+use_log = 1;
 
 if (use_log)
-  %log = 'data/log_ahrs_bug';
-  log = 'data/log_ahrs_still';
+  log = 'data/log_ahrs_bug';
+  %log = 'data/log_ahrs_still';
   %log = 'data/log_ahrs_roll';
   %log = 'data/log_ahrs_yaw';
   %log = 'data/log_ahrs_yaw_pitched';
   [gyro, accel, mag] = read_imu_log(log);
   t = 0:dt:(length(gyro)-1)*dt;
 else
-  nb_samples = 3500;
+  nb_samples = 1500;
   [t, rates, quat] = synth_data(dt, nb_samples);
   [gyro, accel, mag] =  synth_imu(rates, quat);
 end
 
-tilt_status = 0;                      % uninit
+% initialisation
+tilt_status = 0;
 nb_init = 180;
 m_gyro = [ mean(gyro(1, 1:nb_init))
 	   mean(gyro(2, 1:nb_init))
@@ -26,7 +27,7 @@ m_gyro = [ mean(gyro(1, 1:nb_init))
 m_accel = [ mean(accel(1, 1:nb_init))
 	    mean(accel(2, 1:nb_init))
 	    mean(accel(3, 1:nb_init)) ];
-%[angle, bias] = 
+
 tilt(tilt_status, m_gyro, m_accel);
 
 [n, m] = size(gyro);
@@ -39,6 +40,7 @@ for idx = 1:m
   end
   [theta_est(idx), bias(idx)] = tilt(tilt_status, gyro(:,idx), accel(:,idx));
   theta_measure(idx) = theta_of_accel(accel(:,idx));
+%  theta_measure(idx) = phi_of_accel(accel(:,idx));
 end;
 
 subplot(3,1,1)

sw/logalizer/matlab/tilt.m

@@ -1,3 +1,9 @@
+%
+% this is a 2 states kalman filter used to fuse the readings of a
+% two axis accelerometer and one axis gyro.
+% The filter estimates the angle and the gyro bias.
+%
+%
 function [angle, bias, rate] = tilt(status, gyro, accel)
 
 
@@ -10,23 +16,21 @@ persistent tilt_bias;  %
 persistent tilt_rate;  % unbiased rate
 persistent tilt_P;     % covariance matrix
 
-tilt_dt = 0.015625;    % time step
+tilt_dt = 0.015625;    % prediction time step
 tilt_R =  0.3;         % measurement covariance noise
                        % means we expect a 0.3 rad jitter from the
                        % accelerometer
-
-		       
 		       
 if (status == TILT_UNINIT)
   [tilt_angle, tilt_bias, tilt_rate, tilt_P] = tilt_init(gyro, accel);
 else
-  [tilt_angle, tilt_rate, tilt_P] = tilt_predict(gyro, tilt_P, ...
-						 tilt_angle, tilt_bias, ...
-						 tilt_dt);
+  [tilt_angle, tilt_rate, tilt_P] = ...
+      tilt_predict(gyro, tilt_P, ...
+		   tilt_angle, tilt_bias, tilt_rate, ...
+		   tilt_dt);
   if (status == TILT_UPDATE)
     [tilt_angle, tilt_bias, tilt_P] = tilt_update(accel, tilt_R, tilt_P, ...
-						  tilt_angle, ...
-						  tilt_bias);
+						  tilt_angle, tilt_bias);
   end
 end		       
 
@@ -42,6 +46,7 @@ rate = tilt_rate;
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 function [angle, bias, rate, P] = tilt_init(gyro, accel)
 angle = theta_of_accel(accel);
+%angle = phi_of_accel(accel);
 
 bias = gyro(2);
 
@@ -59,12 +64,13 @@ P = [ 1 0
 %
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 function [angle_out, rate_out, P_out] = tilt_predict(gyro, P_in, ...
-						     angle_in,  bias, ...
+						     angle_in,  bias, rate,...
 						     dt)
 rate_out = gyro(2) - bias;
+%rate_out = gyro(1) - bias;
 
 % update state ( X += Xdot * dt )
-angle_out = angle_in + rate_out * dt;
+angle_out = angle_in + (rate + rate_out) / 2 * dt;
 
 % update covariance ( Pdot = A*P + P*A' + Q )
 %                   ( P += Pdot * dt        ) 
@@ -98,6 +104,7 @@ function [angle_out, bias_out, P_out] = tilt_update(accel, R, P_in, ...
 						    bias_in)
 
 measure_angle = theta_of_accel(accel);
+%measure_angle = phi_of_accel(accel);
 err = measure_angle - angle_in;
 
 C = [ 1 0 ];