*** empty log message ***

sw/in_progress/inertial/scilab/README

@@ -12,3 +12,6 @@ ahrs_euler_ekf_seqm:
 
 2/ ahrs_quat
 
+ahrs_quat_ekf_compm:
+	quaternion + bias estimation using ekf filter and complete measurements ( all 3 euler angles for every update)
+

sw/in_progress/inertial/scilab/ahrs_euler_ekf_compm.sce

@@ -45,16 +45,16 @@ Q = [ Qe   0   0   0   0   0
        0   0   0   0  Qb   0
        0   0   0   0   0  Qb ];
 
-// measure covariance noise
+// measurement covariance noise
 R = [
       1.3^2  0.     0.  
       0.     1.3^2  0.
       0.     0.     2.5^2
     ];
 
-X=[X0];
+X=[X0];       // state
-P=[P0];
+P=[P0];       // state covariance
-M=[X0(1:3)];
+M=[X0(1:3)];  // measurements
 
 for i=2:length(time)
   // command
@@ -89,4 +89,4 @@ for i=2:length(time)
   
 end
 
-ahrsh_euler_display(time, X, P, M)
+ahrs_euler_display(time, X, P, M)

sw/in_progress/inertial/scilab/ahrs_euler_ekf_seqm.sce

@@ -113,4 +113,4 @@ for i=2:length(time)
   
 end
 
-ahrsh_euler_display(time, X, P, M)
+ahrs_euler_display(time, X, P, M)

sw/in_progress/inertial/scilab/ahrs_euler_utils.sci

@@ -39,7 +39,7 @@ endfunction
 //
 //
 //
-function [] = ahrsh_euler_display(time, X, P, M)
+function [] = ahrs_euler_display(time, X, P, M)
   xbasc();
   subplot(3,1,1)
   xtitle('Angle');

sw/in_progress/inertial/scilab/ahrs_quat_utils.sci

@@ -1,8 +1,3 @@
-
-
-
-
-
 //
 //
 //
@@ -12,21 +7,76 @@
 //
 function [X0] = ahrs_quat_init(avg_len, accel, mag, gyro) 
 
- [AE0] = ahrs_euler_init(avg_len, accel, mag, gyro);
+ [AEX0] = ahrs_euler_init(avg_len, accel, mag, gyro);
 
- [quat] = quat_of_euler(AE0(1:3));
+ [quat] = quat_of_euler(AEX0(1:3));
 
- X0 = [ quat AE0(4:6) ]';
+ X0 = [ quat; AEX0(4:6) ];
 
 endfunction
 
 
+//
+//
+//
+// Filter 
+//
+//
+//
+
+function [Pi] = ahrs_quat_get_P(P, i)
+  Pi = P(:, 1+7*(i-1):7*i);
+endfunction
 
 
+//
+//
+//
+// Time derivative of state
+//
+//
+//
+function [Xdot] = ahrs_quat_get_Xdot(X, rate)
+  p = rate(1);
+  q = rate(2);
+  r = rate(3);
+  OMEGA =   1/2 * [ 0  -p  -q  -r  0  0  0
+                    p   0   r  -q  0  0  0
+                    q  -r   0   p  0  0  0
+                    r   q  -p   0  0  0  0
+		    0   0   0   0  0  0  0
+		    0   0   0   0  0  0  0
+		    0   0   0   0  0  0  0 ];
+  Xdot = OMEGA * X;
+endfunction
 
 
+//
+//
+//
+// Derivative of Xdot wrt X
+//
+//
+//
+function [F] = ahrs_quat_get_F(X, rate)
+  q0 = X(1);
+  q1 = X(2);
+  q2 = X(3);
+  q3 = X(4);
 
+  p = rate(1);
+  q = rate(2);
+  r = rate(3);
 
+  F =   1/2 * [ 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;
+                0   0   0   0    0   0   0;
+                0   0   0   0    0   0   0;
+                0   0   0   0    0   0   0 ];
+	  
+endfunction
 
 
 //
@@ -56,13 +106,6 @@ H = [
 
 endfunction
 
-
-
-
-
-
-
-
 function  [H] = ahrs_quat_get_dtheta_dX(X)
 
 DCM = dcm_of_quat(X(1:4));
@@ -102,3 +145,51 @@ H = [
     ]';
 
 endfunction
+
+function [H] = ahrs_quat_compm_get_deuler_dX(X)
+
+Hphi =  ahrs_quat_get_dphi_dX(X);
+Htheta =  ahrs_quat_get_dtheta_dX(X);
+Hpsi =  ahrs_quat_get_dpsi_dX(X);
+H = [ Hphi; Htheta; Hpsi];
+
+endfunction
+
+//
+//
+//
+// Display 
+//
+//
+//
+function [] = ahrs_quat_display(time, X, P, M)
+  xbasc();
+  subplot(3,1,1)
+  xtitle('Angle');
+  EstEulerDeg = [];
+  for i=1:length(time)
+    ead = euler_of_quat(X(1:4, i)) * 180 / %pi;
+    EstEulerDeg = [EstEulerDeg  ead];
+  end  
+  MeasAngleDeg = M * 180 / %pi;
+  plot2d([time; time; time]', EstEulerDeg', style=[5, 3, 2], leg="phi@theta@psi");
+  plot2d([time; time; time]', MeasAngleDeg', style=[0, 0, 0]);
+  
+  subplot(3,1,2)
+  xtitle('Bias');
+  EstBiasDeg = X(5:7,:) * 180 / %pi;
+  plot2d([time; time; time]', EstBiasDeg', style=[5, 3, 2], leg="phi@theta@psi");
+
+  subplot(3,1,3)
+  xtitle('Covariance');
+  
+  Pdiag = [];
+  for i=1:length(time)
+    Pi = ahrs_quat_get_P(P, i);
+    Pdiag = [Pdiag [Pi(1,1) Pi(2,2) Pi(3,3) Pi(4,4) Pi(5,5) Pi(6,6) Pi(7,7)]'];
+  end
+  plot2d([time; time; time; time; time; time; time]', Pdiag', style=[5, 4, 3, 2, 0, 0, 0],... 
+      leg="q0@q1@q2@q3@bp@bq@br");
+
+endfunction
+

sw/in_progress/inertial/scilab/rotations.sci

@@ -22,7 +22,7 @@ q1 = -cosphi2 * sintheta2 * sinpsi2 + sinphi2 * costheta2 * cospsi2;
 q2 =  cosphi2 * sintheta2 * cospsi2 + sinphi2 * costheta2 * sinpsi2;
 q3 =  cosphi2 * costheta2 * sinpsi2 - sinphi2 * sintheta2 * cospsi2;
 
-quat = [q0 q1 q2 q3];
+quat = [q0 q1 q2 q3]';
 
 endfunction
 
@@ -38,13 +38,13 @@ function [euler] = euler_of_quat(quat)
   theta = -asin( dcm02 );
   psi = atan( dcm01, dcm00 );
 
-  euler = [phi theta psi];
+  euler = [phi; theta; psi];
 endfunction
 
 function [X1] = normalise_quat(X0)
  quat = X0(1:4);
  quat = quat / norm(quat);
- X1 = [quat; X0(5:7)];
+ X1 = [quat];
 endfunction