*** empty log message ***

sw/in_progress/inertial/scilab/ahrs_euler_ekf_compm.sce

@@ -5,6 +5,9 @@ getf('ahrs_euler_utils.sci');
 getf('ekf.sci');
 
 use_sim = 1;
+predict_only = 0;
+predict_discrete = 0;
+use_state_for_psi = 0;
 
 if (use_sim),
   rand('seed', 0);
@@ -15,9 +18,9 @@ if (use_sim),
   [accel, mag, gyro] = imu_sim(time, true_rates, true_eulers);
 else
   //filename = "../data/log_ahrs_test";
-  filename = "../data/log_ahrs_bug";
+  //filename = "../data/log_ahrs_bug";
   //filename = "../data/log_ahrs_roll";
-  //filename = "../data/log_ahrs_yaw_pitched";
+  filename = "../data/log_ahrs_yaw_pitched";
   [time, accel, mag, gyro] = imu_read_log(filename);
 end
 
@@ -36,7 +39,7 @@ P0 = [ P0e   0   0   0   0   0
          0   0   0   0   0  P0b ];
 
 // process covariance noise 
-Qe = 0.;
+Qe = 0.0;
 Qb = 0.008;
 
 Q = [ Qe   0   0   0   0   0
@@ -57,7 +60,6 @@ X=[X0];       // state
 P=[P0];       // state covariance
 M=[X0(1:3)];  // measurements
 
-predict_only = 0;
 
 for i=2:length(time)
   // command
@@ -71,9 +73,20 @@ for i=2:length(time)
   // Jacobian of state time derivative wrt state
   Fi_ = ahrs_euler_get_F(Xi_, rate_i_);
    
-  [Xpred, Ppred] = ekf_predict_continuous(Xi_, Xdoti_, dt, Pi_, Fi_, Q);
+  if predict_discrete
- 
+    [Xpred, Ppred] = ekf_predict_discrete(Xi_, Xdoti_, dt, Pi_, Fi_, Q);  
-  measure_i1 = euler_of_accel_mag(accel(:, i), mag(:, i));
+  else
+    [Xpred, Ppred] = ekf_predict_continuous(Xi_, Xdoti_, dt, Pi_, Fi_, Q);
+  end
+  
+  if use_state_for_psi
+    m_phi = phi_of_accel(accel(:, i));
+    m_theta = theta_of_accel(accel(:, i));
+    m_psi = psi_of_mag(Xpred(1), Xpred(2), mag(:, i));
+    measure_i1 = [ m_phi; m_theta; m_psi];
+  else
+    measure_i1 = euler_of_accel_mag(accel(:, i), mag(:, i));
+  end
 
   M = [M measure_i1];
     

sw/in_progress/inertial/scilab/calibrate_imu.sce

@@ -0,0 +1,29 @@
+clear();
+getf('rotations.sci');
+getf('imu.sci');
+
+
+rand('seed', 0);
+getf('quadrotor.sci');
+true_euler0 = [ 0.01; 0.2; 0.4]; 
+dt =  0.015625;
+
+[time, true_rates, true_eulers] = quadrotor_gen_roll_step(true_euler0, dt);
+[accel, mag, gyro] = imu_sim_misaligned(time, true_rates, true_eulers);
+
+
+
+
+xbasc();
+true_rates_deg = deg_of_rad(true_rates);
+subplot(2,1,1)
+plot2d([time]', true_rates_deg', style=[5, 3, 2], leg="rate_p@rate_q@rate_r");
+xtitle( 'True rates', 's', 'deg/s') ;
+
+subplot(2,1,2)
+plot2d([time]', gyro', style=[5, 3, 2], leg="g_x@g_y@g_z");
+xtitle( 'Raw gyros', 's', 'adc') ;
+
+
+
+

sw/in_progress/inertial/scilab/ekf.sci

@@ -24,7 +24,8 @@ endfunction
 function [X1, P1] = ekf_predict_discrete(X0, X0dot, dt, P0, F, Q)
 
 X1 = X0 + X0dot * dt;
-P0dot = F*P0*F' + Q;
+expF = expm(dt * F);
+P0dot = expF*P0*expF' + Q;
 P1 = P0 + P0dot * dt;
 
 endfunction
@@ -43,4 +44,27 @@ K = P0 * H' * inv(E);
 P1 = P0 - K * H * P0;
 X1 = X0 + K * err;
 
-endfunction
+endfunction
+
+
+
+//
+// Pade approximation of matrix exponential
+//
+function [expA] = mat_exp(A, epsilon)
+
+  normA = norm(A, 'inf');
+  //Ns = max(0, int(normA)) ???
+  Ns = normA;
+  ki = 2^(-Ns) * A;
+
+  i=1;
+//  foo = 2^(3-2*i)*
+
+
+expA = A;
+
+
+endfunction
+
+

sw/in_progress/inertial/scilab/imu.sci

@@ -79,6 +79,35 @@ function [accel, mag, gyro] = imu_sim(time, rates, eulers)
 endfunction
 
 
+function [accel, mag, gyro] = imu_sim_misaligned(time, rates, eulers)
+  gyro_supply = 5.;
+  gyro_neutral = [512; 512; 512];
+  gyro_gains = rad_of_deg([ 300/512; 300/512; 300/512]);
+  gyro_sigma_noise = [ 3; 3; 3]; 
+
+  gyro_offset_deg = [0   0   0
+                     0   0   0   
+		     0   0   0 ];
+
+  gyro_orientation = [] ;
+		 
+  gyro_align = [ 1   0 0
+                 0.1 1 0
+	         0   0 1 ];
+
+  accel = [];
+  mag = [];
+  gyro = [];
+  for i=1:length(time),
+    gyro_noise = rand(3, 1, "normal") .* gyro_sigma_noise;
+//    gyro_noise = [ 0; 0; 0];
+    g = gyro_neutral + (gyro_align * rates(:, i)) ./ gyro_gains + gyro_noise;
+    g = round(g);
+    gyro = [gyro g];
+  end
+endfunction
+
+
 //
 //
 //

sw/in_progress/inertial/scilab/rotations.sci

@@ -1,7 +1,22 @@
 //
 //
 //
-// Angles convertion
+// Angle units
+//
+//
+//
+function [rad] = rad_of_deg(deg)
+  rad = deg * %pi / 180; 
+endfunction
+
+function [deg] = deg_of_rad(rad)
+  deg = rad * 180 / %pi;
+endfunction
+
+//
+//
+//
+// Rotation convertion
 //
 //
 //
@@ -41,12 +56,6 @@ function [euler] = euler_of_quat(quat)
   euler = [phi; theta; psi];
 endfunction
 
-function [X1] = normalise_quat(X0)
- quat = X0(1:4);
- quat = quat / norm(quat);
- X1 = [quat];
-endfunction
-
 
 function [DCM] = dcm_of_euler(euler)
   phi = euler(1);
@@ -88,3 +97,11 @@ DCM = [ dcm00 dcm01 dcm02
         dcm20 dcm21 dcm22 ];
 
 endfunction
+
+
+function [X1] = normalise_quat(X0)
+ quat = X0(1:4);
+ quat = quat / norm(quat);
+ X1 = [quat];
+endfunction
+

sw/in_progress/inertial/scilab/tilt_ekf.sce

@@ -1,11 +1,12 @@
 clear();
-
 getf('rotations.sci');
 getf('imu.sci');
 getf('ekf.sci');
 getf('tilt_utils.sci');
 
 use_sim = 1;
+predict_only = 0;
+predict_discrete = 0;
 
 if (use_sim),
   getf('quadrotor.sci');
@@ -42,23 +43,29 @@ for i=1:length(time)-1
         0  8e-3 ];
   X0 = X(:, i);
   P0 = tilt_get_P(P, i);
-  [Xpred, Ppred] = ekf_predict_continuous(X0, X0dot, dt, P0, F, Q);
+  if ( predict_discrete )
-//  [Xpred, Ppred] = ekf_predict_discrete(X0, X0dot, dt, P0, F, Q);
+    [Xpred, Ppred] = ekf_predict_discrete(X0, X0dot, dt, P0, F, Q);
-  
+  else
-//  X = [X Xpred];
+    [Xpred, Ppred] = ekf_predict_continuous(X0, X0dot, dt, P0, F, Q);
-//  P = [P Ppred];
+  end
   
   measure = phi_of_accel(accel(:,i+1));
   M = [M measure];
-  err = measure - Xpred(1);
+
-  // Jacobian of measurement wrt X
+  if ( predict_only )
-  H = [ 1 0 ];
+    X = [X Xpred];
-  // Measurement covariance noise
+    P = [P Ppred];
-  R = 0.3;
+  else
-  [Xup, Pup] = ekf_update(Xpred, Ppred, H, R, err);
+    err = measure - Xpred(1);
+    // Jacobian of measurement wrt X
+    H = [ 1 0 ];
+    // Measurement covariance noise
+    R = 0.3;
+    [Xup, Pup] = ekf_update(Xpred, Ppred, H, R, err);
   
-  X = [X Xup];
+    X = [X Xup];
-  P = [P Pup];
+    P = [P Pup];
+  end
   
 end