Merge pull request rosflight#361 from rosflight/Issue319_ReorgnizeUni…

…tTests

Issue319 reorgnize unit tests

reports/estimator.tex

@@ -88,7 +88,7 @@ \subsection{Accelerometers}
 
 \begin{figure}[h]
 \centering
-\includegraphics[width=0.5\textwidth]{fig/accelerometer_measurement.png}
+% \includegraphics[width=0.5\textwidth]{fig/accelerometer_measurement.png}
 \caption{Illustration of acceleromemeter measurement if MAV is at steady state.  Because the accelerometer does not measure gravity, if we assume that the MAV is not accelerating, then the only accelerations measured are those which counteract gravity.  The angle $\phi$ can be therefore calculated using the vector $y_{acc}$}
 \label{fig:accelerometer_measure}
 

scripts/plot_estimator_test.py

@@ -0,0 +1,51 @@
+import numpy as np
+import matplotlib.pyplot as plt
+
+stateType = np.dtype([
+	('t', np.float64),
+	('q', (np.float64,4)),
+	('qhat', (np.float64,4)),
+	('err', (np.float64,3)),
+	('eulerErr', (np.float64,3)),
+	('bias', (np.float32,3))
+])
+
+def plotResults(filename):
+	data = np.fromfile("../test/build/"+filename, dtype=stateType)
+
+	plt.figure(figsize=[12,9])
+	plt.suptitle(filename)
+	for i in range(4):
+		plt.subplot(4, 3, 3*i+1)
+		plt.plot(data['t'], data['q'][:,i], label="q")
+		plt.plot(data['t'], data['qhat'][:,i], label="qhat")
+		if i == 0:
+			plt.legend()
+
+	for i in range(3):
+		plt.subplot(4,3,3*i+2)
+		plt.plot(data['t'], data['err'][:,i])
+	plt.subplot(4,3,11)
+	err_norm = np.sqrt(np.sum(np.square(data['err']),axis=1))
+	plt.plot(data['t'], err_norm)
+
+	for i in range(3):
+		plt.subplot(4,3,3*i+3)
+		plt.plot(data['t'], data['bias'][:,i])
+
+
+	print("{} max error: {}".format(filename, np.max(err_norm)))
+
+
+if __name__ == '__main__':
+	plotResults("linearGyro.bin")
+	plotResults("quadGyro.bin")
+	plotResults("expInt.bin")
+	plotResults("expQuadInt.bin")
+	plotResults("acc.bin")
+	plotResults("estState.bin")
+	plotResults("estBias.bin")
+	plotResults("estStateExtAtt.bin")
+	# plotResults("movingExtAtt.bin")
+
+	plt.show()

src/estimator.cpp

@@ -167,6 +167,12 @@ void Estimator::run()
   float a_sqrd_norm = accel_LPF_.sqrd_norm();
 
   turbomath::Vector w_acc;
+  if (attitude_correction_next_run_)
+  {
+    attitude_correction_next_run_ = false;
+    w_acc += RF_.params_.get_param_float(PARAM_FILTER_KP_ATT_CORRECTION)*(q_correction_ - state_.attitude);
+  }
+
   if (RF_.params_.get_param_int(PARAM_FILTER_USE_ACC)
       && a_sqrd_norm < 1.1f*1.1f*9.80665f*9.80665f && a_sqrd_norm > 0.9f*0.9f*9.80665f*9.80665f)
   {
@@ -182,29 +188,16 @@ void Estimator::run()
     turbomath::Quaternion q_tilde = q_acc_inv * state_.attitude;
     // Correction Term of Eq. 47a and 47b Mahony Paper
     // w_acc = 2*s_tilde*v_tilde
-    w_acc.x = -2.0f*q_tilde.w*q_tilde.x;
-    w_acc.y = -2.0f*q_tilde.w*q_tilde.y;
-    w_acc.z = 0.0f; // Don't correct z, because it's unobservable from the accelerometer
-
-    // integrate biases from accelerometer feedback
-    // (eq 47b Mahony Paper, using correction term w_acc found above
-    bias_.x -= ki*w_acc.x*dt;
-    bias_.y -= ki*w_acc.y*dt;
-    bias_.z = 0.0;  // Don't integrate z bias, because it's unobservable
-  }
-  else
-  {
-    w_acc.x = 0.0f;
-    w_acc.y = 0.0f;
-    w_acc.z = 0.0f;
-  }
-
-  if (attitude_correction_next_run_)
-  {
-    attitude_correction_next_run_ = false;
-    w_acc += RF_.params_.get_param_float(PARAM_FILTER_KP_ATT_CORRECTION)*(q_correction_ - state_.attitude);
+    w_acc.x += -2.0f*q_tilde.w*q_tilde.x;
+    w_acc.y += -2.0f*q_tilde.w*q_tilde.y;
+    w_acc.z += 0.0f; // Don't correct z, because it's unobservable from the accelerometer
   }
 
+  // integrate biases from accelerometer feedback
+  // (eq 47b Mahony Paper, using correction term w_acc found above
+  bias_.x -= ki*w_acc.x*dt;
+  bias_.y -= ki*w_acc.y*dt;
+  bias_.z -= ki*w_acc.z*dt;
 
   // Handle Gyro Measurements
   turbomath::Vector wbar;