mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-25 00:31:36 +08:00
EKF: Convert magnetic field observation methods to use SymPy generated code (#879)
* EKF: Add comparison test for mag field fusion generated code * EKF: convert mag field fusion to use SymPy generated code * EKF: Add test comparison program for yaw fusion equations * Stop setting 0 to 0 * Reduce if/else statement to only if * EKF: more accurate implementation for sequential fusion of magnetometer data * test: update change indicator * Use matrix::SparseVector<float, 24, ...> for observation jacobian * Adapt the auto code generation to allow for different bracket styles * Add auto generated code for mag fusion * Add generic computation of KHP * Apply generic computation of KHP to mag fusion * tests: update change indicator * tests: update change indicator Co-authored-by: kamilritz <kritz@ethz.ch>
This commit is contained in:
Paul Riseborough
GitHub
@@ -52,6 +52,8 @@ public:
|
||||
typedef matrix::SquareMatrix<float, _k_num_states> SquareMatrix24f;
|
||||
typedef matrix::SquareMatrix<float, 2> Matrix2f;
|
||||
typedef matrix::Vector<float, 4> Vector4f;
|
||||
template<int ... Idxs>
|
||||
using SparseVector24f = matrix::SparseVectorf<24, Idxs...>;
|
||||
|
||||
Ekf() = default;
|
||||
virtual ~Ekf() = default;
|
||||
@@ -664,6 +666,29 @@ private:
|
||||
|
||||
Vector3f getVisionVelocityVarianceInEkfFrame() const;
|
||||
|
||||
// matrix vector multiplication for computing K<24,1> * H<1,24> * P<24,24>
|
||||
// that is optimized by exploring the sparsity in H
|
||||
template <size_t ...Idxs>
|
||||
SquareMatrix24f computeKHP(const Vector24f& K, const SparseVector24f<Idxs...>& H) const {
|
||||
SquareMatrix24f KHP;
|
||||
float KH[H.non_zeros()];
|
||||
for (unsigned row = 0; row < _k_num_states; row++) {
|
||||
for(unsigned i = 0; i < H.non_zeros(); i++) {
|
||||
KH[i] = K(row) * H.atCompressedIndex(i);
|
||||
}
|
||||
|
||||
for (unsigned column = 0; column < _k_num_states; column++) {
|
||||
float tmp = 0.f;
|
||||
for(unsigned i = 0; i < H.non_zeros(); i++) {
|
||||
const size_t index = H.index(i);
|
||||
tmp += KH[i] * P(index, column);
|
||||
}
|
||||
KHP(row,column) = tmp;
|
||||
}
|
||||
}
|
||||
return KHP;
|
||||
}
|
||||
|
||||
// if the covariance correction will result in a negative variance, then
|
||||
// the covariance matrix is unhealthy and must be corrected
|
||||
bool checkAndFixCovarianceUpdate(const SquareMatrix24f& KHP);
|
||||
|
||||
+397
-418
File diff suppressed because one or more lines are too long
@@ -27,26 +27,20 @@ class CodeGenerator:
|
||||
write_string = write_string + "\n\n"
|
||||
self.file.write(write_string)
|
||||
|
||||
def write_matrix(self, matrix, identifier, is_symmetric=False):
|
||||
def write_matrix(self, matrix, variable_name, is_symmetric=False, pre_bracket="(", post_bracket=")"):
|
||||
write_string = ""
|
||||
|
||||
if matrix.shape[0] * matrix.shape[1] == 1:
|
||||
write_string = write_string + identifier + " = " + self.get_ccode(matrix[0]) + ";\n"
|
||||
write_string = write_string + variable_name + " = " + self.get_ccode(matrix[0]) + ";\n"
|
||||
elif matrix.shape[0] == 1 or matrix.shape[1] == 1:
|
||||
for i in range(0,len(matrix)):
|
||||
if (identifier == "Kfusion"):
|
||||
# Vector f format used by Kfusion
|
||||
write_string = write_string + identifier + "(" + str(i) + ") = " + self.get_ccode(matrix[i]) + ";\n"
|
||||
else:
|
||||
# legacy array format used by Hfusion
|
||||
write_string = write_string + identifier + "[" + str(i) + "] = " + self.get_ccode(matrix[i]) + ";\n"
|
||||
write_string = write_string + variable_name + pre_bracket + str(i) + post_bracket + " = " + self.get_ccode(matrix[i]) + ";\n"
|
||||
|
||||
else:
|
||||
for j in range(0, matrix.shape[1]):
|
||||
for i in range(0, matrix.shape[0]):
|
||||
if j >= i or not is_symmetric:
|
||||
write_string = write_string + identifier + "(" + str(i) + "," + str(j) + ") = " + self.get_ccode(matrix[i,j]) + ";\n"
|
||||
# legacy array format
|
||||
# write_string = write_string + identifier + "[" + str(i) + "][" + str(j) + "] = " + self.get_ccode(matrix[i,j]) + ";\n"
|
||||
write_string = write_string + variable_name + pre_bracket + str(i) + "," + str(j) + post_bracket + " = " + self.get_ccode(matrix[i,j]) + ";\n"
|
||||
|
||||
write_string = write_string + "\n\n"
|
||||
self.file.write(write_string)
|
||||
|
||||
File diff suppressed because one or more lines are too long
@@ -0,0 +1,249 @@
|
||||
// Axis 0 equations
|
||||
// Sub Expressions
|
||||
const float HKX0 = -magD*q2 + magE*q3 + magN*q0;
|
||||
const float HKX1 = magD*q3 + magE*q2 + magN*q1;
|
||||
const float HKX2 = magE*q1;
|
||||
const float HKX3 = magD*q0;
|
||||
const float HKX4 = magN*q2;
|
||||
const float HKX5 = magD*q1 + magE*q0 - magN*q3;
|
||||
const float HKX6 = powf(q0, 2) + powf(q1, 2) - powf(q2, 2) - powf(q3, 2);
|
||||
const float HKX7 = q0*q3 + q1*q2;
|
||||
const float HKX8 = q1*q3;
|
||||
const float HKX9 = q0*q2;
|
||||
const float HKX10 = 2*HKX7;
|
||||
const float HKX11 = -2*HKX8 + 2*HKX9;
|
||||
const float HKX12 = 2*HKX1;
|
||||
const float HKX13 = 2*HKX0;
|
||||
const float HKX14 = -2*HKX2 + 2*HKX3 + 2*HKX4;
|
||||
const float HKX15 = 2*HKX5;
|
||||
const float HKX16 = HKX10*P(0,17) - HKX11*P(0,18) + HKX12*P(0,1) + HKX13*P(0,0) - HKX14*P(0,2) + HKX15*P(0,3) + HKX6*P(0,16) + P(0,19);
|
||||
const float HKX17 = HKX10*P(16,17) - HKX11*P(16,18) + HKX12*P(1,16) + HKX13*P(0,16) - HKX14*P(2,16) + HKX15*P(3,16) + HKX6*P(16,16) + P(16,19);
|
||||
const float HKX18 = HKX10*P(17,18) - HKX11*P(18,18) + HKX12*P(1,18) + HKX13*P(0,18) - HKX14*P(2,18) + HKX15*P(3,18) + HKX6*P(16,18) + P(18,19);
|
||||
const float HKX19 = HKX10*P(2,17) - HKX11*P(2,18) + HKX12*P(1,2) + HKX13*P(0,2) - HKX14*P(2,2) + HKX15*P(2,3) + HKX6*P(2,16) + P(2,19);
|
||||
const float HKX20 = HKX10*P(17,17) - HKX11*P(17,18) + HKX12*P(1,17) + HKX13*P(0,17) - HKX14*P(2,17) + HKX15*P(3,17) + HKX6*P(16,17) + P(17,19);
|
||||
const float HKX21 = HKX10*P(3,17) - HKX11*P(3,18) + HKX12*P(1,3) + HKX13*P(0,3) - HKX14*P(2,3) + HKX15*P(3,3) + HKX6*P(3,16) + P(3,19);
|
||||
const float HKX22 = HKX10*P(1,17) - HKX11*P(1,18) + HKX12*P(1,1) + HKX13*P(0,1) - HKX14*P(1,2) + HKX15*P(1,3) + HKX6*P(1,16) + P(1,19);
|
||||
const float HKX23 = HKX10*P(17,19) - HKX11*P(18,19) + HKX12*P(1,19) + HKX13*P(0,19) - HKX14*P(2,19) + HKX15*P(3,19) + HKX6*P(16,19) + P(19,19);
|
||||
const float HKX24 = 1.0F/(HKX10*HKX20 - HKX11*HKX18 + HKX12*HKX22 + HKX13*HKX16 - HKX14*HKX19 + HKX15*HKX21 + HKX17*HKX6 + HKX23 + R_MAG);
|
||||
|
||||
|
||||
// Observation Jacobians
|
||||
Hfusion.at<0>() = 2*HKX0;
|
||||
Hfusion.at<1>() = 2*HKX1;
|
||||
Hfusion.at<2>() = 2*HKX2 - 2*HKX3 - 2*HKX4;
|
||||
Hfusion.at<3>() = 2*HKX5;
|
||||
Hfusion.at<4>() = 0;
|
||||
Hfusion.at<5>() = 0;
|
||||
Hfusion.at<6>() = 0;
|
||||
Hfusion.at<7>() = 0;
|
||||
Hfusion.at<8>() = 0;
|
||||
Hfusion.at<9>() = 0;
|
||||
Hfusion.at<10>() = 0;
|
||||
Hfusion.at<11>() = 0;
|
||||
Hfusion.at<12>() = 0;
|
||||
Hfusion.at<13>() = 0;
|
||||
Hfusion.at<14>() = 0;
|
||||
Hfusion.at<15>() = 0;
|
||||
Hfusion.at<16>() = HKX6;
|
||||
Hfusion.at<17>() = 2*HKX7;
|
||||
Hfusion.at<18>() = 2*HKX8 - 2*HKX9;
|
||||
Hfusion.at<19>() = 1;
|
||||
Hfusion.at<20>() = 0;
|
||||
Hfusion.at<21>() = 0;
|
||||
Hfusion.at<22>() = 0;
|
||||
Hfusion.at<23>() = 0;
|
||||
|
||||
|
||||
// Kalman gains
|
||||
Kfusion(0) = HKX16*HKX24;
|
||||
Kfusion(1) = HKX22*HKX24;
|
||||
Kfusion(2) = HKX19*HKX24;
|
||||
Kfusion(3) = HKX21*HKX24;
|
||||
Kfusion(4) = HKX24*(HKX10*P(4,17) - HKX11*P(4,18) + HKX12*P(1,4) + HKX13*P(0,4) - HKX14*P(2,4) + HKX15*P(3,4) + HKX6*P(4,16) + P(4,19));
|
||||
Kfusion(5) = HKX24*(HKX10*P(5,17) - HKX11*P(5,18) + HKX12*P(1,5) + HKX13*P(0,5) - HKX14*P(2,5) + HKX15*P(3,5) + HKX6*P(5,16) + P(5,19));
|
||||
Kfusion(6) = HKX24*(HKX10*P(6,17) - HKX11*P(6,18) + HKX12*P(1,6) + HKX13*P(0,6) - HKX14*P(2,6) + HKX15*P(3,6) + HKX6*P(6,16) + P(6,19));
|
||||
Kfusion(7) = HKX24*(HKX10*P(7,17) - HKX11*P(7,18) + HKX12*P(1,7) + HKX13*P(0,7) - HKX14*P(2,7) + HKX15*P(3,7) + HKX6*P(7,16) + P(7,19));
|
||||
Kfusion(8) = HKX24*(HKX10*P(8,17) - HKX11*P(8,18) + HKX12*P(1,8) + HKX13*P(0,8) - HKX14*P(2,8) + HKX15*P(3,8) + HKX6*P(8,16) + P(8,19));
|
||||
Kfusion(9) = HKX24*(HKX10*P(9,17) - HKX11*P(9,18) + HKX12*P(1,9) + HKX13*P(0,9) - HKX14*P(2,9) + HKX15*P(3,9) + HKX6*P(9,16) + P(9,19));
|
||||
Kfusion(10) = HKX24*(HKX10*P(10,17) - HKX11*P(10,18) + HKX12*P(1,10) + HKX13*P(0,10) - HKX14*P(2,10) + HKX15*P(3,10) + HKX6*P(10,16) + P(10,19));
|
||||
Kfusion(11) = HKX24*(HKX10*P(11,17) - HKX11*P(11,18) + HKX12*P(1,11) + HKX13*P(0,11) - HKX14*P(2,11) + HKX15*P(3,11) + HKX6*P(11,16) + P(11,19));
|
||||
Kfusion(12) = HKX24*(HKX10*P(12,17) - HKX11*P(12,18) + HKX12*P(1,12) + HKX13*P(0,12) - HKX14*P(2,12) + HKX15*P(3,12) + HKX6*P(12,16) + P(12,19));
|
||||
Kfusion(13) = HKX24*(HKX10*P(13,17) - HKX11*P(13,18) + HKX12*P(1,13) + HKX13*P(0,13) - HKX14*P(2,13) + HKX15*P(3,13) + HKX6*P(13,16) + P(13,19));
|
||||
Kfusion(14) = HKX24*(HKX10*P(14,17) - HKX11*P(14,18) + HKX12*P(1,14) + HKX13*P(0,14) - HKX14*P(2,14) + HKX15*P(3,14) + HKX6*P(14,16) + P(14,19));
|
||||
Kfusion(15) = HKX24*(HKX10*P(15,17) - HKX11*P(15,18) + HKX12*P(1,15) + HKX13*P(0,15) - HKX14*P(2,15) + HKX15*P(3,15) + HKX6*P(15,16) + P(15,19));
|
||||
Kfusion(16) = HKX17*HKX24;
|
||||
Kfusion(17) = HKX20*HKX24;
|
||||
Kfusion(18) = HKX18*HKX24;
|
||||
Kfusion(19) = HKX23*HKX24;
|
||||
Kfusion(20) = HKX24*(HKX10*P(17,20) - HKX11*P(18,20) + HKX12*P(1,20) + HKX13*P(0,20) - HKX14*P(2,20) + HKX15*P(3,20) + HKX6*P(16,20) + P(19,20));
|
||||
Kfusion(21) = HKX24*(HKX10*P(17,21) - HKX11*P(18,21) + HKX12*P(1,21) + HKX13*P(0,21) - HKX14*P(2,21) + HKX15*P(3,21) + HKX6*P(16,21) + P(19,21));
|
||||
Kfusion(22) = HKX24*(HKX10*P(17,22) - HKX11*P(18,22) + HKX12*P(1,22) + HKX13*P(0,22) - HKX14*P(2,22) + HKX15*P(3,22) + HKX6*P(16,22) + P(19,22));
|
||||
Kfusion(23) = HKX24*(HKX10*P(17,23) - HKX11*P(18,23) + HKX12*P(1,23) + HKX13*P(0,23) - HKX14*P(2,23) + HKX15*P(3,23) + HKX6*P(16,23) + P(19,23));
|
||||
|
||||
|
||||
// Axis 1 equations
|
||||
// Sub Expressions
|
||||
const float HKY0 = magD*q1 + magE*q0 - magN*q3;
|
||||
const float HKY1 = magD*q0 - magE*q1 + magN*q2;
|
||||
const float HKY2 = magD*q3 + magE*q2 + magN*q1;
|
||||
const float HKY3 = magD*q2;
|
||||
const float HKY4 = magE*q3;
|
||||
const float HKY5 = magN*q0;
|
||||
const float HKY6 = q1*q2;
|
||||
const float HKY7 = q0*q3;
|
||||
const float HKY8 = powf(q0, 2) - powf(q1, 2) + powf(q2, 2) - powf(q3, 2);
|
||||
const float HKY9 = q0*q1 + q2*q3;
|
||||
const float HKY10 = 2*HKY9;
|
||||
const float HKY11 = -2*HKY6 + 2*HKY7;
|
||||
const float HKY12 = 2*HKY2;
|
||||
const float HKY13 = 2*HKY0;
|
||||
const float HKY14 = 2*HKY1;
|
||||
const float HKY15 = -2*HKY3 + 2*HKY4 + 2*HKY5;
|
||||
const float HKY16 = HKY10*P(0,18) - HKY11*P(0,16) + HKY12*P(0,2) + HKY13*P(0,0) + HKY14*P(0,1) - HKY15*P(0,3) + HKY8*P(0,17) + P(0,20);
|
||||
const float HKY17 = HKY10*P(17,18) - HKY11*P(16,17) + HKY12*P(2,17) + HKY13*P(0,17) + HKY14*P(1,17) - HKY15*P(3,17) + HKY8*P(17,17) + P(17,20);
|
||||
const float HKY18 = HKY10*P(16,18) - HKY11*P(16,16) + HKY12*P(2,16) + HKY13*P(0,16) + HKY14*P(1,16) - HKY15*P(3,16) + HKY8*P(16,17) + P(16,20);
|
||||
const float HKY19 = HKY10*P(3,18) - HKY11*P(3,16) + HKY12*P(2,3) + HKY13*P(0,3) + HKY14*P(1,3) - HKY15*P(3,3) + HKY8*P(3,17) + P(3,20);
|
||||
const float HKY20 = HKY10*P(18,18) - HKY11*P(16,18) + HKY12*P(2,18) + HKY13*P(0,18) + HKY14*P(1,18) - HKY15*P(3,18) + HKY8*P(17,18) + P(18,20);
|
||||
const float HKY21 = HKY10*P(1,18) - HKY11*P(1,16) + HKY12*P(1,2) + HKY13*P(0,1) + HKY14*P(1,1) - HKY15*P(1,3) + HKY8*P(1,17) + P(1,20);
|
||||
const float HKY22 = HKY10*P(2,18) - HKY11*P(2,16) + HKY12*P(2,2) + HKY13*P(0,2) + HKY14*P(1,2) - HKY15*P(2,3) + HKY8*P(2,17) + P(2,20);
|
||||
const float HKY23 = HKY10*P(18,20) - HKY11*P(16,20) + HKY12*P(2,20) + HKY13*P(0,20) + HKY14*P(1,20) - HKY15*P(3,20) + HKY8*P(17,20) + P(20,20);
|
||||
const float HKY24 = 1.0F/(HKY10*HKY20 - HKY11*HKY18 + HKY12*HKY22 + HKY13*HKY16 + HKY14*HKY21 - HKY15*HKY19 + HKY17*HKY8 + HKY23 + R_MAG);
|
||||
|
||||
|
||||
// Observation Jacobians
|
||||
Hfusion.at<0>() = 2*HKY0;
|
||||
Hfusion.at<1>() = 2*HKY1;
|
||||
Hfusion.at<2>() = 2*HKY2;
|
||||
Hfusion.at<3>() = 2*HKY3 - 2*HKY4 - 2*HKY5;
|
||||
Hfusion.at<4>() = 0;
|
||||
Hfusion.at<5>() = 0;
|
||||
Hfusion.at<6>() = 0;
|
||||
Hfusion.at<7>() = 0;
|
||||
Hfusion.at<8>() = 0;
|
||||
Hfusion.at<9>() = 0;
|
||||
Hfusion.at<10>() = 0;
|
||||
Hfusion.at<11>() = 0;
|
||||
Hfusion.at<12>() = 0;
|
||||
Hfusion.at<13>() = 0;
|
||||
Hfusion.at<14>() = 0;
|
||||
Hfusion.at<15>() = 0;
|
||||
Hfusion.at<16>() = 2*HKY6 - 2*HKY7;
|
||||
Hfusion.at<17>() = HKY8;
|
||||
Hfusion.at<18>() = 2*HKY9;
|
||||
Hfusion.at<19>() = 0;
|
||||
Hfusion.at<20>() = 1;
|
||||
Hfusion.at<21>() = 0;
|
||||
Hfusion.at<22>() = 0;
|
||||
Hfusion.at<23>() = 0;
|
||||
|
||||
|
||||
// Kalman gains
|
||||
Kfusion(0) = HKY16*HKY24;
|
||||
Kfusion(1) = HKY21*HKY24;
|
||||
Kfusion(2) = HKY22*HKY24;
|
||||
Kfusion(3) = HKY19*HKY24;
|
||||
Kfusion(4) = HKY24*(HKY10*P(4,18) - HKY11*P(4,16) + HKY12*P(2,4) + HKY13*P(0,4) + HKY14*P(1,4) - HKY15*P(3,4) + HKY8*P(4,17) + P(4,20));
|
||||
Kfusion(5) = HKY24*(HKY10*P(5,18) - HKY11*P(5,16) + HKY12*P(2,5) + HKY13*P(0,5) + HKY14*P(1,5) - HKY15*P(3,5) + HKY8*P(5,17) + P(5,20));
|
||||
Kfusion(6) = HKY24*(HKY10*P(6,18) - HKY11*P(6,16) + HKY12*P(2,6) + HKY13*P(0,6) + HKY14*P(1,6) - HKY15*P(3,6) + HKY8*P(6,17) + P(6,20));
|
||||
Kfusion(7) = HKY24*(HKY10*P(7,18) - HKY11*P(7,16) + HKY12*P(2,7) + HKY13*P(0,7) + HKY14*P(1,7) - HKY15*P(3,7) + HKY8*P(7,17) + P(7,20));
|
||||
Kfusion(8) = HKY24*(HKY10*P(8,18) - HKY11*P(8,16) + HKY12*P(2,8) + HKY13*P(0,8) + HKY14*P(1,8) - HKY15*P(3,8) + HKY8*P(8,17) + P(8,20));
|
||||
Kfusion(9) = HKY24*(HKY10*P(9,18) - HKY11*P(9,16) + HKY12*P(2,9) + HKY13*P(0,9) + HKY14*P(1,9) - HKY15*P(3,9) + HKY8*P(9,17) + P(9,20));
|
||||
Kfusion(10) = HKY24*(HKY10*P(10,18) - HKY11*P(10,16) + HKY12*P(2,10) + HKY13*P(0,10) + HKY14*P(1,10) - HKY15*P(3,10) + HKY8*P(10,17) + P(10,20));
|
||||
Kfusion(11) = HKY24*(HKY10*P(11,18) - HKY11*P(11,16) + HKY12*P(2,11) + HKY13*P(0,11) + HKY14*P(1,11) - HKY15*P(3,11) + HKY8*P(11,17) + P(11,20));
|
||||
Kfusion(12) = HKY24*(HKY10*P(12,18) - HKY11*P(12,16) + HKY12*P(2,12) + HKY13*P(0,12) + HKY14*P(1,12) - HKY15*P(3,12) + HKY8*P(12,17) + P(12,20));
|
||||
Kfusion(13) = HKY24*(HKY10*P(13,18) - HKY11*P(13,16) + HKY12*P(2,13) + HKY13*P(0,13) + HKY14*P(1,13) - HKY15*P(3,13) + HKY8*P(13,17) + P(13,20));
|
||||
Kfusion(14) = HKY24*(HKY10*P(14,18) - HKY11*P(14,16) + HKY12*P(2,14) + HKY13*P(0,14) + HKY14*P(1,14) - HKY15*P(3,14) + HKY8*P(14,17) + P(14,20));
|
||||
Kfusion(15) = HKY24*(HKY10*P(15,18) - HKY11*P(15,16) + HKY12*P(2,15) + HKY13*P(0,15) + HKY14*P(1,15) - HKY15*P(3,15) + HKY8*P(15,17) + P(15,20));
|
||||
Kfusion(16) = HKY18*HKY24;
|
||||
Kfusion(17) = HKY17*HKY24;
|
||||
Kfusion(18) = HKY20*HKY24;
|
||||
Kfusion(19) = HKY24*(HKY10*P(18,19) - HKY11*P(16,19) + HKY12*P(2,19) + HKY13*P(0,19) + HKY14*P(1,19) - HKY15*P(3,19) + HKY8*P(17,19) + P(19,20));
|
||||
Kfusion(20) = HKY23*HKY24;
|
||||
Kfusion(21) = HKY24*(HKY10*P(18,21) - HKY11*P(16,21) + HKY12*P(2,21) + HKY13*P(0,21) + HKY14*P(1,21) - HKY15*P(3,21) + HKY8*P(17,21) + P(20,21));
|
||||
Kfusion(22) = HKY24*(HKY10*P(18,22) - HKY11*P(16,22) + HKY12*P(2,22) + HKY13*P(0,22) + HKY14*P(1,22) - HKY15*P(3,22) + HKY8*P(17,22) + P(20,22));
|
||||
Kfusion(23) = HKY24*(HKY10*P(18,23) - HKY11*P(16,23) + HKY12*P(2,23) + HKY13*P(0,23) + HKY14*P(1,23) - HKY15*P(3,23) + HKY8*P(17,23) + P(20,23));
|
||||
|
||||
|
||||
// Axis 2 equations
|
||||
// Sub Expressions
|
||||
const float HKZ0 = magD*q0 - magE*q1 + magN*q2;
|
||||
const float HKZ1 = magN*q3;
|
||||
const float HKZ2 = magD*q1;
|
||||
const float HKZ3 = magE*q0;
|
||||
const float HKZ4 = -magD*q2 + magE*q3 + magN*q0;
|
||||
const float HKZ5 = magD*q3 + magE*q2 + magN*q1;
|
||||
const float HKZ6 = q0*q2 + q1*q3;
|
||||
const float HKZ7 = q2*q3;
|
||||
const float HKZ8 = q0*q1;
|
||||
const float HKZ9 = powf(q0, 2) - powf(q1, 2) - powf(q2, 2) + powf(q3, 2);
|
||||
const float HKZ10 = 2*HKZ6;
|
||||
const float HKZ11 = -2*HKZ7 + 2*HKZ8;
|
||||
const float HKZ12 = 2*HKZ5;
|
||||
const float HKZ13 = 2*HKZ0;
|
||||
const float HKZ14 = -2*HKZ1 + 2*HKZ2 + 2*HKZ3;
|
||||
const float HKZ15 = 2*HKZ4;
|
||||
const float HKZ16 = HKZ10*P(0,16) - HKZ11*P(0,17) + HKZ12*P(0,3) + HKZ13*P(0,0) - HKZ14*P(0,1) + HKZ15*P(0,2) + HKZ9*P(0,18) + P(0,21);
|
||||
const float HKZ17 = HKZ10*P(16,18) - HKZ11*P(17,18) + HKZ12*P(3,18) + HKZ13*P(0,18) - HKZ14*P(1,18) + HKZ15*P(2,18) + HKZ9*P(18,18) + P(18,21);
|
||||
const float HKZ18 = HKZ10*P(16,17) - HKZ11*P(17,17) + HKZ12*P(3,17) + HKZ13*P(0,17) - HKZ14*P(1,17) + HKZ15*P(2,17) + HKZ9*P(17,18) + P(17,21);
|
||||
const float HKZ19 = HKZ10*P(1,16) - HKZ11*P(1,17) + HKZ12*P(1,3) + HKZ13*P(0,1) - HKZ14*P(1,1) + HKZ15*P(1,2) + HKZ9*P(1,18) + P(1,21);
|
||||
const float HKZ20 = HKZ10*P(16,16) - HKZ11*P(16,17) + HKZ12*P(3,16) + HKZ13*P(0,16) - HKZ14*P(1,16) + HKZ15*P(2,16) + HKZ9*P(16,18) + P(16,21);
|
||||
const float HKZ21 = HKZ10*P(3,16) - HKZ11*P(3,17) + HKZ12*P(3,3) + HKZ13*P(0,3) - HKZ14*P(1,3) + HKZ15*P(2,3) + HKZ9*P(3,18) + P(3,21);
|
||||
const float HKZ22 = HKZ10*P(2,16) - HKZ11*P(2,17) + HKZ12*P(2,3) + HKZ13*P(0,2) - HKZ14*P(1,2) + HKZ15*P(2,2) + HKZ9*P(2,18) + P(2,21);
|
||||
const float HKZ23 = HKZ10*P(16,21) - HKZ11*P(17,21) + HKZ12*P(3,21) + HKZ13*P(0,21) - HKZ14*P(1,21) + HKZ15*P(2,21) + HKZ9*P(18,21) + P(21,21);
|
||||
const float HKZ24 = 1.0F/(HKZ10*HKZ20 - HKZ11*HKZ18 + HKZ12*HKZ21 + HKZ13*HKZ16 - HKZ14*HKZ19 + HKZ15*HKZ22 + HKZ17*HKZ9 + HKZ23 + R_MAG);
|
||||
|
||||
|
||||
// Observation Jacobians
|
||||
Hfusion.at<0>() = 2*HKZ0;
|
||||
Hfusion.at<1>() = 2*HKZ1 - 2*HKZ2 - 2*HKZ3;
|
||||
Hfusion.at<2>() = 2*HKZ4;
|
||||
Hfusion.at<3>() = 2*HKZ5;
|
||||
Hfusion.at<4>() = 0;
|
||||
Hfusion.at<5>() = 0;
|
||||
Hfusion.at<6>() = 0;
|
||||
Hfusion.at<7>() = 0;
|
||||
Hfusion.at<8>() = 0;
|
||||
Hfusion.at<9>() = 0;
|
||||
Hfusion.at<10>() = 0;
|
||||
Hfusion.at<11>() = 0;
|
||||
Hfusion.at<12>() = 0;
|
||||
Hfusion.at<13>() = 0;
|
||||
Hfusion.at<14>() = 0;
|
||||
Hfusion.at<15>() = 0;
|
||||
Hfusion.at<16>() = 2*HKZ6;
|
||||
Hfusion.at<17>() = 2*HKZ7 - 2*HKZ8;
|
||||
Hfusion.at<18>() = HKZ9;
|
||||
Hfusion.at<19>() = 0;
|
||||
Hfusion.at<20>() = 0;
|
||||
Hfusion.at<21>() = 1;
|
||||
Hfusion.at<22>() = 0;
|
||||
Hfusion.at<23>() = 0;
|
||||
|
||||
|
||||
// Kalman gains
|
||||
Kfusion(0) = HKZ16*HKZ24;
|
||||
Kfusion(1) = HKZ19*HKZ24;
|
||||
Kfusion(2) = HKZ22*HKZ24;
|
||||
Kfusion(3) = HKZ21*HKZ24;
|
||||
Kfusion(4) = HKZ24*(HKZ10*P(4,16) - HKZ11*P(4,17) + HKZ12*P(3,4) + HKZ13*P(0,4) - HKZ14*P(1,4) + HKZ15*P(2,4) + HKZ9*P(4,18) + P(4,21));
|
||||
Kfusion(5) = HKZ24*(HKZ10*P(5,16) - HKZ11*P(5,17) + HKZ12*P(3,5) + HKZ13*P(0,5) - HKZ14*P(1,5) + HKZ15*P(2,5) + HKZ9*P(5,18) + P(5,21));
|
||||
Kfusion(6) = HKZ24*(HKZ10*P(6,16) - HKZ11*P(6,17) + HKZ12*P(3,6) + HKZ13*P(0,6) - HKZ14*P(1,6) + HKZ15*P(2,6) + HKZ9*P(6,18) + P(6,21));
|
||||
Kfusion(7) = HKZ24*(HKZ10*P(7,16) - HKZ11*P(7,17) + HKZ12*P(3,7) + HKZ13*P(0,7) - HKZ14*P(1,7) + HKZ15*P(2,7) + HKZ9*P(7,18) + P(7,21));
|
||||
Kfusion(8) = HKZ24*(HKZ10*P(8,16) - HKZ11*P(8,17) + HKZ12*P(3,8) + HKZ13*P(0,8) - HKZ14*P(1,8) + HKZ15*P(2,8) + HKZ9*P(8,18) + P(8,21));
|
||||
Kfusion(9) = HKZ24*(HKZ10*P(9,16) - HKZ11*P(9,17) + HKZ12*P(3,9) + HKZ13*P(0,9) - HKZ14*P(1,9) + HKZ15*P(2,9) + HKZ9*P(9,18) + P(9,21));
|
||||
Kfusion(10) = HKZ24*(HKZ10*P(10,16) - HKZ11*P(10,17) + HKZ12*P(3,10) + HKZ13*P(0,10) - HKZ14*P(1,10) + HKZ15*P(2,10) + HKZ9*P(10,18) + P(10,21));
|
||||
Kfusion(11) = HKZ24*(HKZ10*P(11,16) - HKZ11*P(11,17) + HKZ12*P(3,11) + HKZ13*P(0,11) - HKZ14*P(1,11) + HKZ15*P(2,11) + HKZ9*P(11,18) + P(11,21));
|
||||
Kfusion(12) = HKZ24*(HKZ10*P(12,16) - HKZ11*P(12,17) + HKZ12*P(3,12) + HKZ13*P(0,12) - HKZ14*P(1,12) + HKZ15*P(2,12) + HKZ9*P(12,18) + P(12,21));
|
||||
Kfusion(13) = HKZ24*(HKZ10*P(13,16) - HKZ11*P(13,17) + HKZ12*P(3,13) + HKZ13*P(0,13) - HKZ14*P(1,13) + HKZ15*P(2,13) + HKZ9*P(13,18) + P(13,21));
|
||||
Kfusion(14) = HKZ24*(HKZ10*P(14,16) - HKZ11*P(14,17) + HKZ12*P(3,14) + HKZ13*P(0,14) - HKZ14*P(1,14) + HKZ15*P(2,14) + HKZ9*P(14,18) + P(14,21));
|
||||
Kfusion(15) = HKZ24*(HKZ10*P(15,16) - HKZ11*P(15,17) + HKZ12*P(3,15) + HKZ13*P(0,15) - HKZ14*P(1,15) + HKZ15*P(2,15) + HKZ9*P(15,18) + P(15,21));
|
||||
Kfusion(16) = HKZ20*HKZ24;
|
||||
Kfusion(17) = HKZ18*HKZ24;
|
||||
Kfusion(18) = HKZ17*HKZ24;
|
||||
Kfusion(19) = HKZ24*(HKZ10*P(16,19) - HKZ11*P(17,19) + HKZ12*P(3,19) + HKZ13*P(0,19) - HKZ14*P(1,19) + HKZ15*P(2,19) + HKZ9*P(18,19) + P(19,21));
|
||||
Kfusion(20) = HKZ24*(HKZ10*P(16,20) - HKZ11*P(17,20) + HKZ12*P(3,20) + HKZ13*P(0,20) - HKZ14*P(1,20) + HKZ15*P(2,20) + HKZ9*P(18,20) + P(20,21));
|
||||
Kfusion(21) = HKZ23*HKZ24;
|
||||
Kfusion(22) = HKZ24*(HKZ10*P(16,22) - HKZ11*P(17,22) + HKZ12*P(3,22) + HKZ13*P(0,22) - HKZ14*P(1,22) + HKZ15*P(2,22) + HKZ9*P(18,22) + P(21,22));
|
||||
Kfusion(23) = HKZ24*(HKZ10*P(16,23) - HKZ11*P(17,23) + HKZ12*P(3,23) + HKZ13*P(0,23) - HKZ14*P(1,23) + HKZ15*P(2,23) + HKZ9*P(18,23) + P(21,23));
|
||||
|
||||
|
||||
@@ -0,0 +1,250 @@
|
||||
// Sub Expressions
|
||||
const float HK0 = magE*q3;
|
||||
const float HK1 = magN*q0;
|
||||
const float HK2 = magD*q2;
|
||||
const float HK3 = HK0 + HK1 - HK2;
|
||||
const float HK4 = 2*HK3;
|
||||
const float HK5 = magD*q3 + magE*q2 + magN*q1;
|
||||
const float HK6 = 2*HK5;
|
||||
const float HK7 = magE*q1;
|
||||
const float HK8 = magD*q0;
|
||||
const float HK9 = magN*q2;
|
||||
const float HK10 = magD*q1;
|
||||
const float HK11 = magE*q0;
|
||||
const float HK12 = magN*q3;
|
||||
const float HK13 = HK10 + HK11 - HK12;
|
||||
const float HK14 = 2*HK13;
|
||||
const float HK15 = powf(q1, 2);
|
||||
const float HK16 = powf(q2, 2);
|
||||
const float HK17 = -HK16;
|
||||
const float HK18 = powf(q0, 2);
|
||||
const float HK19 = powf(q3, 2);
|
||||
const float HK20 = HK18 - HK19;
|
||||
const float HK21 = HK15 + HK17 + HK20;
|
||||
const float HK22 = q0*q3;
|
||||
const float HK23 = q1*q2;
|
||||
const float HK24 = HK22 + HK23;
|
||||
const float HK25 = q1*q3;
|
||||
const float HK26 = q0*q2;
|
||||
const float HK27 = 2*HK24;
|
||||
const float HK28 = -2*HK25 + 2*HK26;
|
||||
const float HK29 = 2*HK5;
|
||||
const float HK30 = 2*HK3;
|
||||
const float HK31 = -HK7 + HK8 + HK9;
|
||||
const float HK32 = 2*HK31;
|
||||
const float HK33 = HK32*P(0,2);
|
||||
const float HK34 = 2*HK13;
|
||||
const float HK35 = HK34*P(0,3);
|
||||
const float HK36 = HK21*P(0,16) + HK27*P(0,17) - HK28*P(0,18) + HK29*P(0,1) + HK30*P(0,0) - HK33 + HK35 + P(0,19);
|
||||
const float HK37 = HK21*P(16,16) + HK27*P(16,17) - HK28*P(16,18) + HK29*P(1,16) + HK30*P(0,16) - HK32*P(2,16) + HK34*P(3,16) + P(16,19);
|
||||
const float HK38 = HK21*P(16,18) + HK27*P(17,18) - HK28*P(18,18) + HK29*P(1,18) + HK30*P(0,18) - HK32*P(2,18) + HK34*P(3,18) + P(18,19);
|
||||
const float HK39 = HK29*P(1,2);
|
||||
const float HK40 = HK30*P(0,2);
|
||||
const float HK41 = HK21*P(2,16) + HK27*P(2,17) - HK28*P(2,18) - HK32*P(2,2) + HK34*P(2,3) + HK39 + HK40 + P(2,19);
|
||||
const float HK42 = HK21*P(16,17) + HK27*P(17,17) - HK28*P(17,18) + HK29*P(1,17) + HK30*P(0,17) - HK32*P(2,17) + HK34*P(3,17) + P(17,19);
|
||||
const float HK43 = HK29*P(1,3);
|
||||
const float HK44 = HK30*P(0,3);
|
||||
const float HK45 = HK21*P(3,16) + HK27*P(3,17) - HK28*P(3,18) - HK32*P(2,3) + HK34*P(3,3) + HK43 + HK44 + P(3,19);
|
||||
const float HK46 = HK32*P(1,2);
|
||||
const float HK47 = HK34*P(1,3);
|
||||
const float HK48 = HK21*P(1,16) + HK27*P(1,17) - HK28*P(1,18) + HK29*P(1,1) + HK30*P(0,1) - HK46 + HK47 + P(1,19);
|
||||
const float HK49 = HK21*P(16,19) + HK27*P(17,19) - HK28*P(18,19) + HK29*P(1,19) + HK30*P(0,19) - HK32*P(2,19) + HK34*P(3,19) + P(19,19);
|
||||
const float HK50 = 1.0F/(HK21*HK37 + HK27*HK42 - HK28*HK38 + HK29*HK48 + HK30*HK36 - HK32*HK41 + HK34*HK45 + HK49 + R_MAG);
|
||||
const float HK51 = 2*HK31;
|
||||
const float HK52 = -HK15;
|
||||
const float HK53 = HK16 + HK20 + HK52;
|
||||
const float HK54 = q0*q1;
|
||||
const float HK55 = q2*q3;
|
||||
const float HK56 = HK54 + HK55;
|
||||
const float HK57 = 2*HK56;
|
||||
const float HK58 = 2*HK22 - 2*HK23;
|
||||
const float HK59 = HK32*P(0,1);
|
||||
const float HK60 = HK29*P(0,2) + HK34*P(0,0) - HK44 + HK53*P(0,17) + HK57*P(0,18) - HK58*P(0,16) + HK59 + P(0,20);
|
||||
const float HK61 = HK29*P(2,17) - HK30*P(3,17) + HK32*P(1,17) + HK34*P(0,17) + HK53*P(17,17) + HK57*P(17,18) - HK58*P(16,17) + P(17,20);
|
||||
const float HK62 = HK29*P(2,16) - HK30*P(3,16) + HK32*P(1,16) + HK34*P(0,16) + HK53*P(16,17) + HK57*P(16,18) - HK58*P(16,16) + P(16,20);
|
||||
const float HK63 = HK29*P(2,3);
|
||||
const float HK64 = -HK30*P(3,3) + HK32*P(1,3) + HK35 + HK53*P(3,17) + HK57*P(3,18) - HK58*P(3,16) + HK63 + P(3,20);
|
||||
const float HK65 = HK29*P(2,18) - HK30*P(3,18) + HK32*P(1,18) + HK34*P(0,18) + HK53*P(17,18) + HK57*P(18,18) - HK58*P(16,18) + P(18,20);
|
||||
const float HK66 = HK34*P(0,1);
|
||||
const float HK67 = -HK30*P(1,3) + HK32*P(1,1) + HK39 + HK53*P(1,17) + HK57*P(1,18) - HK58*P(1,16) + HK66 + P(1,20);
|
||||
const float HK68 = HK30*P(2,3);
|
||||
const float HK69 = HK29*P(2,2) + HK34*P(0,2) + HK46 + HK53*P(2,17) + HK57*P(2,18) - HK58*P(2,16) - HK68 + P(2,20);
|
||||
const float HK70 = HK29*P(2,20) - HK30*P(3,20) + HK32*P(1,20) + HK34*P(0,20) + HK53*P(17,20) + HK57*P(18,20) - HK58*P(16,20) + P(20,20);
|
||||
const float HK71 = 1.0F/(HK29*HK69 - HK30*HK64 + HK32*HK67 + HK34*HK60 + HK53*HK61 + HK57*HK65 - HK58*HK62 + HK70 + R_MAG);
|
||||
const float HK72 = HK25 + HK26;
|
||||
const float HK73 = HK17 + HK18 + HK19 + HK52;
|
||||
const float HK74 = 2*HK72;
|
||||
const float HK75 = 2*HK54 - 2*HK55;
|
||||
const float HK76 = HK29*P(0,3) + HK32*P(0,0) + HK40 - HK66 + HK73*P(0,18) + HK74*P(0,16) - HK75*P(0,17) + P(0,21);
|
||||
const float HK77 = HK29*P(3,18) + HK30*P(2,18) + HK32*P(0,18) - HK34*P(1,18) + HK73*P(18,18) + HK74*P(16,18) - HK75*P(17,18) + P(18,21);
|
||||
const float HK78 = HK29*P(3,17) + HK30*P(2,17) + HK32*P(0,17) - HK34*P(1,17) + HK73*P(17,18) + HK74*P(16,17) - HK75*P(17,17) + P(17,21);
|
||||
const float HK79 = HK30*P(1,2) - HK34*P(1,1) + HK43 + HK59 + HK73*P(1,18) + HK74*P(1,16) - HK75*P(1,17) + P(1,21);
|
||||
const float HK80 = HK29*P(3,16) + HK30*P(2,16) + HK32*P(0,16) - HK34*P(1,16) + HK73*P(16,18) + HK74*P(16,16) - HK75*P(16,17) + P(16,21);
|
||||
const float HK81 = HK29*P(3,3) + HK32*P(0,3) - HK47 + HK68 + HK73*P(3,18) + HK74*P(3,16) - HK75*P(3,17) + P(3,21);
|
||||
const float HK82 = HK30*P(2,2) + HK33 - HK34*P(1,2) + HK63 + HK73*P(2,18) + HK74*P(2,16) - HK75*P(2,17) + P(2,21);
|
||||
const float HK83 = HK29*P(3,21) + HK30*P(2,21) + HK32*P(0,21) - HK34*P(1,21) + HK73*P(18,21) + HK74*P(16,21) - HK75*P(17,21) + P(21,21);
|
||||
const float HK84 = 1.0F/(HK29*HK81 + HK30*HK82 + HK32*HK76 - HK34*HK79 + HK73*HK77 + HK74*HK80 - HK75*HK78 + HK83 + R_MAG);
|
||||
|
||||
|
||||
// Observation Jacobians - axis 0
|
||||
Hfusion.at<0>() = HK4;
|
||||
Hfusion.at<1>() = HK6;
|
||||
Hfusion.at<2>() = 2*HK7 - 2*HK8 - 2*HK9;
|
||||
Hfusion.at<3>() = HK14;
|
||||
Hfusion.at<4>() = 0;
|
||||
Hfusion.at<5>() = 0;
|
||||
Hfusion.at<6>() = 0;
|
||||
Hfusion.at<7>() = 0;
|
||||
Hfusion.at<8>() = 0;
|
||||
Hfusion.at<9>() = 0;
|
||||
Hfusion.at<10>() = 0;
|
||||
Hfusion.at<11>() = 0;
|
||||
Hfusion.at<12>() = 0;
|
||||
Hfusion.at<13>() = 0;
|
||||
Hfusion.at<14>() = 0;
|
||||
Hfusion.at<15>() = 0;
|
||||
Hfusion.at<16>() = HK21;
|
||||
Hfusion.at<17>() = 2*HK24;
|
||||
Hfusion.at<18>() = 2*HK25 - 2*HK26;
|
||||
Hfusion.at<19>() = 1;
|
||||
Hfusion.at<20>() = 0;
|
||||
Hfusion.at<21>() = 0;
|
||||
Hfusion.at<22>() = 0;
|
||||
Hfusion.at<23>() = 0;
|
||||
|
||||
|
||||
// Kalman gains - axis 0
|
||||
Kfusion(0) = HK36*HK50;
|
||||
Kfusion(1) = HK48*HK50;
|
||||
Kfusion(2) = HK41*HK50;
|
||||
Kfusion(3) = HK45*HK50;
|
||||
Kfusion(4) = HK50*(HK21*P(4,16) + HK27*P(4,17) - HK28*P(4,18) + HK29*P(1,4) + HK30*P(0,4) - HK32*P(2,4) + HK34*P(3,4) + P(4,19));
|
||||
Kfusion(5) = HK50*(HK21*P(5,16) + HK27*P(5,17) - HK28*P(5,18) + HK29*P(1,5) + HK30*P(0,5) - HK32*P(2,5) + HK34*P(3,5) + P(5,19));
|
||||
Kfusion(6) = HK50*(HK21*P(6,16) + HK27*P(6,17) - HK28*P(6,18) + HK29*P(1,6) + HK30*P(0,6) - HK32*P(2,6) + HK34*P(3,6) + P(6,19));
|
||||
Kfusion(7) = HK50*(HK21*P(7,16) + HK27*P(7,17) - HK28*P(7,18) + HK29*P(1,7) + HK30*P(0,7) - HK32*P(2,7) + HK34*P(3,7) + P(7,19));
|
||||
Kfusion(8) = HK50*(HK21*P(8,16) + HK27*P(8,17) - HK28*P(8,18) + HK29*P(1,8) + HK30*P(0,8) - HK32*P(2,8) + HK34*P(3,8) + P(8,19));
|
||||
Kfusion(9) = HK50*(HK21*P(9,16) + HK27*P(9,17) - HK28*P(9,18) + HK29*P(1,9) + HK30*P(0,9) - HK32*P(2,9) + HK34*P(3,9) + P(9,19));
|
||||
Kfusion(10) = HK50*(HK21*P(10,16) + HK27*P(10,17) - HK28*P(10,18) + HK29*P(1,10) + HK30*P(0,10) - HK32*P(2,10) + HK34*P(3,10) + P(10,19));
|
||||
Kfusion(11) = HK50*(HK21*P(11,16) + HK27*P(11,17) - HK28*P(11,18) + HK29*P(1,11) + HK30*P(0,11) - HK32*P(2,11) + HK34*P(3,11) + P(11,19));
|
||||
Kfusion(12) = HK50*(HK21*P(12,16) + HK27*P(12,17) - HK28*P(12,18) + HK29*P(1,12) + HK30*P(0,12) - HK32*P(2,12) + HK34*P(3,12) + P(12,19));
|
||||
Kfusion(13) = HK50*(HK21*P(13,16) + HK27*P(13,17) - HK28*P(13,18) + HK29*P(1,13) + HK30*P(0,13) - HK32*P(2,13) + HK34*P(3,13) + P(13,19));
|
||||
Kfusion(14) = HK50*(HK21*P(14,16) + HK27*P(14,17) - HK28*P(14,18) + HK29*P(1,14) + HK30*P(0,14) - HK32*P(2,14) + HK34*P(3,14) + P(14,19));
|
||||
Kfusion(15) = HK50*(HK21*P(15,16) + HK27*P(15,17) - HK28*P(15,18) + HK29*P(1,15) + HK30*P(0,15) - HK32*P(2,15) + HK34*P(3,15) + P(15,19));
|
||||
Kfusion(16) = HK37*HK50;
|
||||
Kfusion(17) = HK42*HK50;
|
||||
Kfusion(18) = HK38*HK50;
|
||||
Kfusion(19) = HK49*HK50;
|
||||
Kfusion(20) = HK50*(HK21*P(16,20) + HK27*P(17,20) - HK28*P(18,20) + HK29*P(1,20) + HK30*P(0,20) - HK32*P(2,20) + HK34*P(3,20) + P(19,20));
|
||||
Kfusion(21) = HK50*(HK21*P(16,21) + HK27*P(17,21) - HK28*P(18,21) + HK29*P(1,21) + HK30*P(0,21) - HK32*P(2,21) + HK34*P(3,21) + P(19,21));
|
||||
Kfusion(22) = HK50*(HK21*P(16,22) + HK27*P(17,22) - HK28*P(18,22) + HK29*P(1,22) + HK30*P(0,22) - HK32*P(2,22) + HK34*P(3,22) + P(19,22));
|
||||
Kfusion(23) = HK50*(HK21*P(16,23) + HK27*P(17,23) - HK28*P(18,23) + HK29*P(1,23) + HK30*P(0,23) - HK32*P(2,23) + HK34*P(3,23) + P(19,23));
|
||||
|
||||
|
||||
// Observation Jacobians - axis 1
|
||||
Hfusion.at<0>() = HK14;
|
||||
Hfusion.at<1>() = HK51;
|
||||
Hfusion.at<2>() = HK6;
|
||||
Hfusion.at<3>() = -2*HK0 - 2*HK1 + 2*HK2;
|
||||
Hfusion.at<4>() = 0;
|
||||
Hfusion.at<5>() = 0;
|
||||
Hfusion.at<6>() = 0;
|
||||
Hfusion.at<7>() = 0;
|
||||
Hfusion.at<8>() = 0;
|
||||
Hfusion.at<9>() = 0;
|
||||
Hfusion.at<10>() = 0;
|
||||
Hfusion.at<11>() = 0;
|
||||
Hfusion.at<12>() = 0;
|
||||
Hfusion.at<13>() = 0;
|
||||
Hfusion.at<14>() = 0;
|
||||
Hfusion.at<15>() = 0;
|
||||
Hfusion.at<16>() = -2*HK22 + 2*HK23;
|
||||
Hfusion.at<17>() = HK53;
|
||||
Hfusion.at<18>() = 2*HK56;
|
||||
Hfusion.at<19>() = 0;
|
||||
Hfusion.at<20>() = 1;
|
||||
Hfusion.at<21>() = 0;
|
||||
Hfusion.at<22>() = 0;
|
||||
Hfusion.at<23>() = 0;
|
||||
|
||||
|
||||
// Kalman gains - axis 1
|
||||
Kfusion(0) = HK60*HK71;
|
||||
Kfusion(1) = HK67*HK71;
|
||||
Kfusion(2) = HK69*HK71;
|
||||
Kfusion(3) = HK64*HK71;
|
||||
Kfusion(4) = HK71*(HK29*P(2,4) - HK30*P(3,4) + HK32*P(1,4) + HK34*P(0,4) + HK53*P(4,17) + HK57*P(4,18) - HK58*P(4,16) + P(4,20));
|
||||
Kfusion(5) = HK71*(HK29*P(2,5) - HK30*P(3,5) + HK32*P(1,5) + HK34*P(0,5) + HK53*P(5,17) + HK57*P(5,18) - HK58*P(5,16) + P(5,20));
|
||||
Kfusion(6) = HK71*(HK29*P(2,6) - HK30*P(3,6) + HK32*P(1,6) + HK34*P(0,6) + HK53*P(6,17) + HK57*P(6,18) - HK58*P(6,16) + P(6,20));
|
||||
Kfusion(7) = HK71*(HK29*P(2,7) - HK30*P(3,7) + HK32*P(1,7) + HK34*P(0,7) + HK53*P(7,17) + HK57*P(7,18) - HK58*P(7,16) + P(7,20));
|
||||
Kfusion(8) = HK71*(HK29*P(2,8) - HK30*P(3,8) + HK32*P(1,8) + HK34*P(0,8) + HK53*P(8,17) + HK57*P(8,18) - HK58*P(8,16) + P(8,20));
|
||||
Kfusion(9) = HK71*(HK29*P(2,9) - HK30*P(3,9) + HK32*P(1,9) + HK34*P(0,9) + HK53*P(9,17) + HK57*P(9,18) - HK58*P(9,16) + P(9,20));
|
||||
Kfusion(10) = HK71*(HK29*P(2,10) - HK30*P(3,10) + HK32*P(1,10) + HK34*P(0,10) + HK53*P(10,17) + HK57*P(10,18) - HK58*P(10,16) + P(10,20));
|
||||
Kfusion(11) = HK71*(HK29*P(2,11) - HK30*P(3,11) + HK32*P(1,11) + HK34*P(0,11) + HK53*P(11,17) + HK57*P(11,18) - HK58*P(11,16) + P(11,20));
|
||||
Kfusion(12) = HK71*(HK29*P(2,12) - HK30*P(3,12) + HK32*P(1,12) + HK34*P(0,12) + HK53*P(12,17) + HK57*P(12,18) - HK58*P(12,16) + P(12,20));
|
||||
Kfusion(13) = HK71*(HK29*P(2,13) - HK30*P(3,13) + HK32*P(1,13) + HK34*P(0,13) + HK53*P(13,17) + HK57*P(13,18) - HK58*P(13,16) + P(13,20));
|
||||
Kfusion(14) = HK71*(HK29*P(2,14) - HK30*P(3,14) + HK32*P(1,14) + HK34*P(0,14) + HK53*P(14,17) + HK57*P(14,18) - HK58*P(14,16) + P(14,20));
|
||||
Kfusion(15) = HK71*(HK29*P(2,15) - HK30*P(3,15) + HK32*P(1,15) + HK34*P(0,15) + HK53*P(15,17) + HK57*P(15,18) - HK58*P(15,16) + P(15,20));
|
||||
Kfusion(16) = HK62*HK71;
|
||||
Kfusion(17) = HK61*HK71;
|
||||
Kfusion(18) = HK65*HK71;
|
||||
Kfusion(19) = HK71*(HK29*P(2,19) - HK30*P(3,19) + HK32*P(1,19) + HK34*P(0,19) + HK53*P(17,19) + HK57*P(18,19) - HK58*P(16,19) + P(19,20));
|
||||
Kfusion(20) = HK70*HK71;
|
||||
Kfusion(21) = HK71*(HK29*P(2,21) - HK30*P(3,21) + HK32*P(1,21) + HK34*P(0,21) + HK53*P(17,21) + HK57*P(18,21) - HK58*P(16,21) + P(20,21));
|
||||
Kfusion(22) = HK71*(HK29*P(2,22) - HK30*P(3,22) + HK32*P(1,22) + HK34*P(0,22) + HK53*P(17,22) + HK57*P(18,22) - HK58*P(16,22) + P(20,22));
|
||||
Kfusion(23) = HK71*(HK29*P(2,23) - HK30*P(3,23) + HK32*P(1,23) + HK34*P(0,23) + HK53*P(17,23) + HK57*P(18,23) - HK58*P(16,23) + P(20,23));
|
||||
|
||||
|
||||
// Observation Jacobians - axis 2
|
||||
Hfusion.at<0>() = HK51;
|
||||
Hfusion.at<1>() = -2*HK10 - 2*HK11 + 2*HK12;
|
||||
Hfusion.at<2>() = HK4;
|
||||
Hfusion.at<3>() = HK6;
|
||||
Hfusion.at<4>() = 0;
|
||||
Hfusion.at<5>() = 0;
|
||||
Hfusion.at<6>() = 0;
|
||||
Hfusion.at<7>() = 0;
|
||||
Hfusion.at<8>() = 0;
|
||||
Hfusion.at<9>() = 0;
|
||||
Hfusion.at<10>() = 0;
|
||||
Hfusion.at<11>() = 0;
|
||||
Hfusion.at<12>() = 0;
|
||||
Hfusion.at<13>() = 0;
|
||||
Hfusion.at<14>() = 0;
|
||||
Hfusion.at<15>() = 0;
|
||||
Hfusion.at<16>() = 2*HK72;
|
||||
Hfusion.at<17>() = -2*HK54 + 2*HK55;
|
||||
Hfusion.at<18>() = HK73;
|
||||
Hfusion.at<19>() = 0;
|
||||
Hfusion.at<20>() = 0;
|
||||
Hfusion.at<21>() = 1;
|
||||
Hfusion.at<22>() = 0;
|
||||
Hfusion.at<23>() = 0;
|
||||
|
||||
|
||||
// Kalman gains - axis 2
|
||||
Kfusion(0) = HK76*HK84;
|
||||
Kfusion(1) = HK79*HK84;
|
||||
Kfusion(2) = HK82*HK84;
|
||||
Kfusion(3) = HK81*HK84;
|
||||
Kfusion(4) = HK84*(HK29*P(3,4) + HK30*P(2,4) + HK32*P(0,4) - HK34*P(1,4) + HK73*P(4,18) + HK74*P(4,16) - HK75*P(4,17) + P(4,21));
|
||||
Kfusion(5) = HK84*(HK29*P(3,5) + HK30*P(2,5) + HK32*P(0,5) - HK34*P(1,5) + HK73*P(5,18) + HK74*P(5,16) - HK75*P(5,17) + P(5,21));
|
||||
Kfusion(6) = HK84*(HK29*P(3,6) + HK30*P(2,6) + HK32*P(0,6) - HK34*P(1,6) + HK73*P(6,18) + HK74*P(6,16) - HK75*P(6,17) + P(6,21));
|
||||
Kfusion(7) = HK84*(HK29*P(3,7) + HK30*P(2,7) + HK32*P(0,7) - HK34*P(1,7) + HK73*P(7,18) + HK74*P(7,16) - HK75*P(7,17) + P(7,21));
|
||||
Kfusion(8) = HK84*(HK29*P(3,8) + HK30*P(2,8) + HK32*P(0,8) - HK34*P(1,8) + HK73*P(8,18) + HK74*P(8,16) - HK75*P(8,17) + P(8,21));
|
||||
Kfusion(9) = HK84*(HK29*P(3,9) + HK30*P(2,9) + HK32*P(0,9) - HK34*P(1,9) + HK73*P(9,18) + HK74*P(9,16) - HK75*P(9,17) + P(9,21));
|
||||
Kfusion(10) = HK84*(HK29*P(3,10) + HK30*P(2,10) + HK32*P(0,10) - HK34*P(1,10) + HK73*P(10,18) + HK74*P(10,16) - HK75*P(10,17) + P(10,21));
|
||||
Kfusion(11) = HK84*(HK29*P(3,11) + HK30*P(2,11) + HK32*P(0,11) - HK34*P(1,11) + HK73*P(11,18) + HK74*P(11,16) - HK75*P(11,17) + P(11,21));
|
||||
Kfusion(12) = HK84*(HK29*P(3,12) + HK30*P(2,12) + HK32*P(0,12) - HK34*P(1,12) + HK73*P(12,18) + HK74*P(12,16) - HK75*P(12,17) + P(12,21));
|
||||
Kfusion(13) = HK84*(HK29*P(3,13) + HK30*P(2,13) + HK32*P(0,13) - HK34*P(1,13) + HK73*P(13,18) + HK74*P(13,16) - HK75*P(13,17) + P(13,21));
|
||||
Kfusion(14) = HK84*(HK29*P(3,14) + HK30*P(2,14) + HK32*P(0,14) - HK34*P(1,14) + HK73*P(14,18) + HK74*P(14,16) - HK75*P(14,17) + P(14,21));
|
||||
Kfusion(15) = HK84*(HK29*P(3,15) + HK30*P(2,15) + HK32*P(0,15) - HK34*P(1,15) + HK73*P(15,18) + HK74*P(15,16) - HK75*P(15,17) + P(15,21));
|
||||
Kfusion(16) = HK80*HK84;
|
||||
Kfusion(17) = HK78*HK84;
|
||||
Kfusion(18) = HK77*HK84;
|
||||
Kfusion(19) = HK84*(HK29*P(3,19) + HK30*P(2,19) + HK32*P(0,19) - HK34*P(1,19) + HK73*P(18,19) + HK74*P(16,19) - HK75*P(17,19) + P(19,21));
|
||||
Kfusion(20) = HK84*(HK29*P(3,20) + HK30*P(2,20) + HK32*P(0,20) - HK34*P(1,20) + HK73*P(18,20) + HK74*P(16,20) - HK75*P(17,20) + P(20,21));
|
||||
Kfusion(21) = HK83*HK84;
|
||||
Kfusion(22) = HK84*(HK29*P(3,22) + HK30*P(2,22) + HK32*P(0,22) - HK34*P(1,22) + HK73*P(18,22) + HK74*P(16,22) - HK75*P(17,22) + P(21,22));
|
||||
Kfusion(23) = HK84*(HK29*P(3,23) + HK30*P(2,23) + HK32*P(0,23) - HK34*P(1,23) + HK73*P(18,23) + HK74*P(16,23) - HK75*P(17,23) + P(21,23));
|
||||
|
||||
|
||||
@@ -0,0 +1,43 @@
|
||||
// Sub Expressions
|
||||
const float IV0 = q0*q1;
|
||||
const float IV1 = q2*q3;
|
||||
const float IV2 = 2*IV0 + 2*IV1;
|
||||
const float IV3 = 2*q0*q3 - 2*q1*q2;
|
||||
const float IV4 = 2*magD*q3 + 2*magE*q2 + 2*magN*q1;
|
||||
const float IV5 = 2*magD*q1 + 2*magE*q0 - 2*magN*q3;
|
||||
const float IV6 = 2*magD*q0 - 2*magE*q1 + 2*magN*q2;
|
||||
const float IV7 = -2*magD*q2 + 2*magE*q3 + 2*magN*q0;
|
||||
const float IV8 = powf(q2, 2);
|
||||
const float IV9 = powf(q3, 2);
|
||||
const float IV10 = powf(q0, 2) - powf(q1, 2);
|
||||
const float IV11 = IV10 + IV8 - IV9;
|
||||
const float IV12 = IV7*P(2,3);
|
||||
const float IV13 = IV5*P(0,1);
|
||||
const float IV14 = IV6*P(0,1);
|
||||
const float IV15 = IV4*P(2,3);
|
||||
const float IV16 = 2*q0*q2 + 2*q1*q3;
|
||||
const float IV17 = 2*IV0 - 2*IV1;
|
||||
const float IV18 = IV10 - IV8 + IV9;
|
||||
|
||||
|
||||
// Observation Jacobians - axis 0
|
||||
Hfusion.at<0>() = R_MAG;
|
||||
Hfusion.at<1>() = IV11*P(17,20) + IV11*(IV11*P(17,17) + IV2*P(17,18) - IV3*P(16,17) + IV4*P(2,17) + IV5*P(0,17) + IV6*P(1,17) - IV7*P(3,17) + P(17,20)) + IV2*P(18,20) + IV2*(IV11*P(17,18) + IV2*P(18,18) - IV3*P(16,18) + IV4*P(2,18) + IV5*P(0,18) + IV6*P(1,18) - IV7*P(3,18) + P(18,20)) - IV3*P(16,20) - IV3*(IV11*P(16,17) + IV2*P(16,18) - IV3*P(16,16) + IV4*P(2,16) + IV5*P(0,16) + IV6*P(1,16) - IV7*P(3,16) + P(16,20)) + IV4*P(2,20) + IV4*(IV11*P(2,17) - IV12 + IV2*P(2,18) - IV3*P(2,16) + IV4*P(2,2) + IV5*P(0,2) + IV6*P(1,2) + P(2,20)) + IV5*P(0,20) + IV5*(IV11*P(0,17) + IV14 + IV2*P(0,18) - IV3*P(0,16) + IV4*P(0,2) + IV5*P(0,0) - IV7*P(0,3) + P(0,20)) + IV6*P(1,20) + IV6*(IV11*P(1,17) + IV13 + IV2*P(1,18) - IV3*P(1,16) + IV4*P(1,2) + IV6*P(1,1) - IV7*P(1,3) + P(1,20)) - IV7*P(3,20) - IV7*(IV11*P(3,17) + IV15 + IV2*P(3,18) - IV3*P(3,16) + IV5*P(0,3) + IV6*P(1,3) - IV7*P(3,3) + P(3,20)) + P(20,20) + R_MAG;
|
||||
Hfusion.at<2>() = IV16*P(16,21) + IV16*(IV16*P(16,16) - IV17*P(16,17) + IV18*P(16,18) + IV4*P(3,16) - IV5*P(1,16) + IV6*P(0,16) + IV7*P(2,16) + P(16,21)) - IV17*P(17,21) - IV17*(IV16*P(16,17) - IV17*P(17,17) + IV18*P(17,18) + IV4*P(3,17) - IV5*P(1,17) + IV6*P(0,17) + IV7*P(2,17) + P(17,21)) + IV18*P(18,21) + IV18*(IV16*P(16,18) - IV17*P(17,18) + IV18*P(18,18) + IV4*P(3,18) - IV5*P(1,18) + IV6*P(0,18) + IV7*P(2,18) + P(18,21)) + IV4*P(3,21) + IV4*(IV12 + IV16*P(3,16) - IV17*P(3,17) + IV18*P(3,18) + IV4*P(3,3) - IV5*P(1,3) + IV6*P(0,3) + P(3,21)) - IV5*P(1,21) - IV5*(IV14 + IV16*P(1,16) - IV17*P(1,17) + IV18*P(1,18) + IV4*P(1,3) - IV5*P(1,1) + IV7*P(1,2) + P(1,21)) + IV6*P(0,21) + IV6*(-IV13 + IV16*P(0,16) - IV17*P(0,17) + IV18*P(0,18) + IV4*P(0,3) + IV6*P(0,0) + IV7*P(0,2) + P(0,21)) + IV7*P(2,21) + IV7*(IV15 + IV16*P(2,16) - IV17*P(2,17) + IV18*P(2,18) - IV5*P(1,2) + IV6*P(0,2) + IV7*P(2,2) + P(2,21)) + P(21,21) + R_MAG;
|
||||
|
||||
|
||||
// Kalman gains - axis 0
|
||||
|
||||
|
||||
// Observation Jacobians - axis 1
|
||||
|
||||
|
||||
// Kalman gains - axis 1
|
||||
|
||||
|
||||
// Observation Jacobians - axis 2
|
||||
|
||||
|
||||
// Kalman gains - axis 2
|
||||
|
||||
|
||||
@@ -0,0 +1,227 @@
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <cstdlib>
|
||||
#include "../../../../../matrix/matrix/math.hpp"
|
||||
|
||||
typedef matrix::Vector<float, 24> Vector24f;
|
||||
typedef matrix::SquareMatrix<float, 24> SquareMatrix24f;
|
||||
|
||||
float sq(float in) {
|
||||
return in * in;
|
||||
}
|
||||
|
||||
int main()
|
||||
{
|
||||
// Compare calculation of observation Jacobians and Kalman gains for sympy and matlab generated equations
|
||||
|
||||
float Hfusion[24] = {};
|
||||
Vector24f H_DECL;
|
||||
Vector24f Kfusion;
|
||||
float decl_innov_var;
|
||||
|
||||
const float R_DECL = sq(0.3f);
|
||||
|
||||
const float _gps_yaw_offset = 1.5f;
|
||||
|
||||
// quaternion inputs must be normalised
|
||||
float q0 = 2.0f * ((float)rand() - 0.5f);
|
||||
float q1 = 2.0f * ((float)rand() - 0.5f);
|
||||
float q2 = 2.0f * ((float)rand() - 0.5f);
|
||||
float q3 = 2.0f * ((float)rand() - 0.5f);
|
||||
const float length = sqrtf(sq(q0) + sq(q1) + sq(q2) + sq(q3));
|
||||
q0 /= length;
|
||||
q1 /= length;
|
||||
q2 /= length;
|
||||
q3 /= length;
|
||||
|
||||
const float magN = 0.04f;
|
||||
const float magE = -0.03f;
|
||||
|
||||
const float h_field_min = 1e-3f;
|
||||
|
||||
// create a symmetrical positive dfinite matrix with off diagonals between -1 and 1 and diagonals between 0 and 1
|
||||
SquareMatrix24f P;
|
||||
for (int col=0; col<=23; col++) {
|
||||
for (int row=0; row<=col; row++) {
|
||||
if (row == col) {
|
||||
P(row,col) = (float)rand();
|
||||
} else {
|
||||
P(col,row) = P(row,col) = 2.0f * ((float)rand() - 0.5f);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// First calculate observationjacobians and Kalman gains using sympy generated equations
|
||||
|
||||
// Calculate intermediate variables
|
||||
const float magN_sq = sq(magN);
|
||||
if (magN_sq < sq(h_field_min)) {
|
||||
printf("bad numerical conditioning\n");
|
||||
return 0;
|
||||
}
|
||||
const float HK0 = 1.0F / magN_sq;
|
||||
const float HK1 = HK0*sq(magE) + 1.0F;
|
||||
const float HK2 = 1.0F/HK1;
|
||||
const float HK3 = 1.0F/magN;
|
||||
const float HK4 = HK2*HK3;
|
||||
const float HK5 = HK3*magE;
|
||||
const float HK6 = HK5*P(16,17) - P(17,17);
|
||||
const float HK7 = 1.0F/sq(HK1);
|
||||
const float HK8 = HK5*P(16,16) - P(16,17);
|
||||
const float innovation_variance = -HK0*HK6*HK7 + HK7*HK8*magE/(magN * magN_sq) + R_DECL;
|
||||
float HK9;
|
||||
if (innovation_variance > R_DECL) {
|
||||
HK9 = HK4/innovation_variance;
|
||||
} else {
|
||||
printf("bad numerical conditioning\n");
|
||||
}
|
||||
|
||||
// Calculate the observation Jacobian
|
||||
// Note only 2 terms are non-zero which can be used in matrix operations for calculation of Kalman gains and covariance update to significantly reduce cost
|
||||
Hfusion[16] = -HK0*HK2*magE;
|
||||
Hfusion[17] = HK4;
|
||||
|
||||
// Calculate the Kalman gains
|
||||
for (unsigned row = 0; row <= 15; row++) {
|
||||
Kfusion(row) = -HK9*(HK5*P(row,16) - P(row,17));
|
||||
}
|
||||
|
||||
Kfusion(16) = -HK8*HK9;
|
||||
Kfusion(17) = -HK6*HK9;
|
||||
|
||||
for (unsigned row = 18; row <= 23; row++) {
|
||||
Kfusion(row) = -HK9*(HK5*P(16,row) - P(17,row));
|
||||
}
|
||||
|
||||
// save output and repeat calculation using legacy matlab generated code
|
||||
float Hfusion_sympy[24];
|
||||
Vector24f Kfusion_sympy;
|
||||
for (int row=0; row<24; row++) {
|
||||
Hfusion_sympy[row] = Hfusion[row];
|
||||
Kfusion_sympy(row) = Kfusion(row);
|
||||
}
|
||||
|
||||
// repeat calculation using matlab generated equations
|
||||
// Calculate intermediate variables
|
||||
float t2 = magE*magE;
|
||||
float t3 = magN*magN;
|
||||
float t4 = t2+t3;
|
||||
// if the horizontal magnetic field is too small, this calculation will be badly conditioned
|
||||
if (t4 < h_field_min*h_field_min) {
|
||||
printf("bad numerical conditioning\n");
|
||||
return 0;
|
||||
}
|
||||
float t5 = P(16,16)*t2;
|
||||
float t6 = P(17,17)*t3;
|
||||
float t7 = t2*t2;
|
||||
float t8 = R_DECL*t7;
|
||||
float t9 = t3*t3;
|
||||
float t10 = R_DECL*t9;
|
||||
float t11 = R_DECL*t2*t3*2.0f;
|
||||
float t14 = P(16,17)*magE*magN;
|
||||
float t15 = P(17,16)*magE*magN;
|
||||
float t12 = t5+t6+t8+t10+t11-t14-t15;
|
||||
float t13;
|
||||
if (fabsf(t12) > 1e-6f) {
|
||||
t13 = 1.0f / t12;
|
||||
} else {
|
||||
printf("bad numerical conditioning\n");
|
||||
return 0;
|
||||
}
|
||||
float t18 = magE*magE;
|
||||
float t19 = magN*magN;
|
||||
float t20 = t18+t19;
|
||||
float t21;
|
||||
if (fabsf(t20) > 1e-6f) {
|
||||
t21 = 1.0f/t20;
|
||||
} else {
|
||||
printf("bad numerical conditioning\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Calculate the observation Jacobian
|
||||
// Note only 2 terms are non-zero which can be used in matrix operations for calculation of Kalman gains and covariance update to significantly reduce cost
|
||||
memset(&H_DECL, 0, sizeof(H_DECL));
|
||||
H_DECL(16) = -magE*t21;
|
||||
H_DECL(17) = magN*t21;
|
||||
|
||||
// Calculate the Kalman gains
|
||||
Kfusion(0) = -t4*t13*(P(0,16)*magE-P(0,17)*magN);
|
||||
Kfusion(1) = -t4*t13*(P(1,16)*magE-P(1,17)*magN);
|
||||
Kfusion(2) = -t4*t13*(P(2,16)*magE-P(2,17)*magN);
|
||||
Kfusion(3) = -t4*t13*(P(3,16)*magE-P(3,17)*magN);
|
||||
Kfusion(4) = -t4*t13*(P(4,16)*magE-P(4,17)*magN);
|
||||
Kfusion(5) = -t4*t13*(P(5,16)*magE-P(5,17)*magN);
|
||||
Kfusion(6) = -t4*t13*(P(6,16)*magE-P(6,17)*magN);
|
||||
Kfusion(7) = -t4*t13*(P(7,16)*magE-P(7,17)*magN);
|
||||
Kfusion(8) = -t4*t13*(P(8,16)*magE-P(8,17)*magN);
|
||||
Kfusion(9) = -t4*t13*(P(9,16)*magE-P(9,17)*magN);
|
||||
Kfusion(10) = -t4*t13*(P(10,16)*magE-P(10,17)*magN);
|
||||
Kfusion(11) = -t4*t13*(P(11,16)*magE-P(11,17)*magN);
|
||||
Kfusion(12) = -t4*t13*(P(12,16)*magE-P(12,17)*magN);
|
||||
Kfusion(13) = -t4*t13*(P(13,16)*magE-P(13,17)*magN);
|
||||
Kfusion(14) = -t4*t13*(P(14,16)*magE-P(14,17)*magN);
|
||||
Kfusion(15) = -t4*t13*(P(15,16)*magE-P(15,17)*magN);
|
||||
Kfusion(16) = -t4*t13*(P(16,16)*magE-P(16,17)*magN);
|
||||
Kfusion(17) = -t4*t13*(P(17,16)*magE-P(17,17)*magN);
|
||||
Kfusion(18) = -t4*t13*(P(18,16)*magE-P(18,17)*magN);
|
||||
Kfusion(19) = -t4*t13*(P(19,16)*magE-P(19,17)*magN);
|
||||
Kfusion(20) = -t4*t13*(P(20,16)*magE-P(20,17)*magN);
|
||||
Kfusion(21) = -t4*t13*(P(21,16)*magE-P(21,17)*magN);
|
||||
Kfusion(22) = -t4*t13*(P(22,16)*magE-P(22,17)*magN);
|
||||
Kfusion(23) = -t4*t13*(P(23,16)*magE-P(23,17)*magN);
|
||||
|
||||
// find largest observation variance difference as a fraction of the matlab value
|
||||
float max_diff_fraction = 0.0f;
|
||||
int max_row;
|
||||
float max_old, max_new;
|
||||
for (int row=0; row<24; row++) {
|
||||
float diff_fraction;
|
||||
if (H_DECL(row) != 0.0f) {
|
||||
diff_fraction = fabsf(Hfusion_sympy[row] - H_DECL(row)) / fabsf(H_DECL(row));
|
||||
} else if (Hfusion_sympy[row] != 0.0f) {
|
||||
diff_fraction = fabsf(Hfusion_sympy[row] - H_DECL(row)) / fabsf(Hfusion_sympy[row]);
|
||||
} else {
|
||||
diff_fraction = 0.0f;
|
||||
}
|
||||
if (diff_fraction > max_diff_fraction) {
|
||||
max_diff_fraction = diff_fraction;
|
||||
max_row = row;
|
||||
max_old = H_DECL(row);
|
||||
max_new = Hfusion_sympy[row];
|
||||
}
|
||||
}
|
||||
|
||||
if (max_diff_fraction > 1E-5f) {
|
||||
printf("Fail: Mag Declination Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
|
||||
} else {
|
||||
printf("Pass: Mag Declination Hfusion max diff fraction = %e\n",max_diff_fraction);
|
||||
}
|
||||
|
||||
// find largest Kalman gain difference as a fraction of the matlab value
|
||||
max_diff_fraction = 0.0f;
|
||||
for (int row=0; row<4; row++) {
|
||||
float diff_fraction;
|
||||
if (Kfusion(row) != 0.0f) {
|
||||
diff_fraction = fabsf(Kfusion_sympy(row) - Kfusion(row)) / fabsf(Kfusion(row));
|
||||
} else if (Kfusion_sympy(row) != 0.0f) {
|
||||
diff_fraction = fabsf(Kfusion_sympy(row) - Kfusion(row)) / fabsf(Kfusion_sympy(row));
|
||||
} else {
|
||||
diff_fraction = 0.0f;
|
||||
}
|
||||
if (diff_fraction > max_diff_fraction) {
|
||||
max_diff_fraction = diff_fraction;
|
||||
max_row = row;
|
||||
max_old = Kfusion(row);
|
||||
max_new = Kfusion_sympy(row);
|
||||
}
|
||||
}
|
||||
|
||||
if (max_diff_fraction > 1E-5f) {
|
||||
printf("Fail: Mag Declination Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
|
||||
} else {
|
||||
printf("Pass: Mag Declination Kfusion max diff fraction = %e\n",max_diff_fraction);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,67 @@
|
||||
// Sub Expressions
|
||||
const float HK0 = powf(magN, -2);
|
||||
const float HK1 = HK0*powf(magE, 2) + 1;
|
||||
const float HK2 = 1.0F/HK1;
|
||||
const float HK3 = 1.0F/magN;
|
||||
const float HK4 = HK2*HK3;
|
||||
const float HK5 = HK3*magE;
|
||||
const float HK6 = HK5*P(16,17) - P(17,17);
|
||||
const float HK7 = powf(HK1, -2);
|
||||
const float HK8 = HK5*P(16,16) - P(16,17);
|
||||
const float HK9 = HK4/(-HK0*HK6*HK7 + HK7*HK8*magE/powf(magN, 3) + R_DECL);
|
||||
|
||||
|
||||
// Observation Jacobians
|
||||
Hfusion.at<0>() = 0;
|
||||
Hfusion.at<1>() = 0;
|
||||
Hfusion.at<2>() = 0;
|
||||
Hfusion.at<3>() = 0;
|
||||
Hfusion.at<4>() = 0;
|
||||
Hfusion.at<5>() = 0;
|
||||
Hfusion.at<6>() = 0;
|
||||
Hfusion.at<7>() = 0;
|
||||
Hfusion.at<8>() = 0;
|
||||
Hfusion.at<9>() = 0;
|
||||
Hfusion.at<10>() = 0;
|
||||
Hfusion.at<11>() = 0;
|
||||
Hfusion.at<12>() = 0;
|
||||
Hfusion.at<13>() = 0;
|
||||
Hfusion.at<14>() = 0;
|
||||
Hfusion.at<15>() = 0;
|
||||
Hfusion.at<16>() = -HK0*HK2*magE;
|
||||
Hfusion.at<17>() = HK4;
|
||||
Hfusion.at<18>() = 0;
|
||||
Hfusion.at<19>() = 0;
|
||||
Hfusion.at<20>() = 0;
|
||||
Hfusion.at<21>() = 0;
|
||||
Hfusion.at<22>() = 0;
|
||||
Hfusion.at<23>() = 0;
|
||||
|
||||
|
||||
// Kalman gains
|
||||
Kfusion(0) = -HK9*(HK5*P(0,16) - P(0,17));
|
||||
Kfusion(1) = -HK9*(HK5*P(1,16) - P(1,17));
|
||||
Kfusion(2) = -HK9*(HK5*P(2,16) - P(2,17));
|
||||
Kfusion(3) = -HK9*(HK5*P(3,16) - P(3,17));
|
||||
Kfusion(4) = -HK9*(HK5*P(4,16) - P(4,17));
|
||||
Kfusion(5) = -HK9*(HK5*P(5,16) - P(5,17));
|
||||
Kfusion(6) = -HK9*(HK5*P(6,16) - P(6,17));
|
||||
Kfusion(7) = -HK9*(HK5*P(7,16) - P(7,17));
|
||||
Kfusion(8) = -HK9*(HK5*P(8,16) - P(8,17));
|
||||
Kfusion(9) = -HK9*(HK5*P(9,16) - P(9,17));
|
||||
Kfusion(10) = -HK9*(HK5*P(10,16) - P(10,17));
|
||||
Kfusion(11) = -HK9*(HK5*P(11,16) - P(11,17));
|
||||
Kfusion(12) = -HK9*(HK5*P(12,16) - P(12,17));
|
||||
Kfusion(13) = -HK9*(HK5*P(13,16) - P(13,17));
|
||||
Kfusion(14) = -HK9*(HK5*P(14,16) - P(14,17));
|
||||
Kfusion(15) = -HK9*(HK5*P(15,16) - P(15,17));
|
||||
Kfusion(16) = -HK8*HK9;
|
||||
Kfusion(17) = -HK6*HK9;
|
||||
Kfusion(18) = -HK9*(HK5*P(16,18) - P(17,18));
|
||||
Kfusion(19) = -HK9*(HK5*P(16,19) - P(17,19));
|
||||
Kfusion(20) = -HK9*(HK5*P(16,20) - P(17,20));
|
||||
Kfusion(21) = -HK9*(HK5*P(16,21) - P(17,21));
|
||||
Kfusion(22) = -HK9*(HK5*P(16,22) - P(17,22));
|
||||
Kfusion(23) = -HK9*(HK5*P(16,23) - P(17,23));
|
||||
|
||||
|
||||
@@ -0,0 +1,315 @@
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <cstdlib>
|
||||
#include "../../../../../matrix/matrix/math.hpp"
|
||||
|
||||
typedef matrix::Vector<float, 24> Vector24f;
|
||||
typedef matrix::SquareMatrix<float, 24> SquareMatrix24f;
|
||||
|
||||
float sq(float in) {
|
||||
return in * in;
|
||||
}
|
||||
|
||||
int main()
|
||||
{
|
||||
// Compare calculation of observation Jacobian for sympy and matlab generated equations
|
||||
|
||||
// observation Jacobians
|
||||
float H_YAW[4];
|
||||
|
||||
// quaternion inputs must be normalised
|
||||
float q0 = 2.0f * ((float)rand() - 0.5f);
|
||||
float q1 = 2.0f * ((float)rand() - 0.5f);
|
||||
float q2 = 2.0f * ((float)rand() - 0.5f);
|
||||
float q3 = 2.0f * ((float)rand() - 0.5f);
|
||||
const float length = sqrtf(sq(q0) + sq(q1) + sq(q2) + sq(q3));
|
||||
q0 /= length;
|
||||
q1 /= length;
|
||||
q2 /= length;
|
||||
q3 /= length;
|
||||
|
||||
|
||||
// calculate 321 yaw observation matrix using two computational paths to work around singularities
|
||||
// in calculation of the Jacobians.
|
||||
|
||||
{
|
||||
// This first comparison is for the 321 sequence option A equations that have a singularity when
|
||||
// yaw is at +- 90 deg
|
||||
|
||||
// perform calculation using sympy generated equations
|
||||
const float SA0 = 2*q3;
|
||||
const float SA1 = 2*q2;
|
||||
const float SA2 = SA0*q0 + SA1*q1;
|
||||
const float SA3 = powf(q0, 2) + powf(q1, 2) - powf(q2, 2) - powf(q3, 2);
|
||||
const float SA4 = powf(SA3, -2);
|
||||
const float SA5 = 1.0F/(powf(SA2, 2)*SA4 + 1);
|
||||
const float SA6 = 1.0F/SA3;
|
||||
const float SA7 = SA2*SA4;
|
||||
const float SA8 = 2*SA7;
|
||||
const float SA9 = 2*SA6;
|
||||
|
||||
H_YAW[0] = SA5*(SA0*SA6 - SA8*q0);
|
||||
H_YAW[1] = SA5*(SA1*SA6 - SA8*q1);
|
||||
H_YAW[2] = SA5*(SA1*SA7 + SA9*q1);
|
||||
H_YAW[3] = SA5*(SA0*SA7 + SA9*q0);
|
||||
|
||||
// save output and repeat calculation using legacy matlab generated code
|
||||
float H_YAW_sympy[4];
|
||||
for (int row=0; row<4; row++) {
|
||||
H_YAW_sympy[row] = H_YAW[row];
|
||||
}
|
||||
|
||||
// repeat calculation using matlab generated equations
|
||||
float t9 = q0*q3;
|
||||
float t10 = q1*q2;
|
||||
float t2 = t9+t10;
|
||||
float t3 = q0*q0;
|
||||
float t4 = q1*q1;
|
||||
float t5 = q2*q2;
|
||||
float t6 = q3*q3;
|
||||
float t7 = t3+t4-t5-t6;
|
||||
float t16 = q3*t3;
|
||||
float t17 = q3*t5;
|
||||
float t18 = q0*q1*q2*2.0f;
|
||||
float t19 = t16+t17+t18-q3*t4+q3*t6;
|
||||
float t24 = q2*t4;
|
||||
float t25 = q2*t6;
|
||||
float t26 = q0*q1*q3*2.0f;
|
||||
float t27 = t24+t25+t26-q2*t3+q2*t5;
|
||||
float t28 = q1*t3;
|
||||
float t29 = q1*t5;
|
||||
float t30 = q0*q2*q3*2.0f;
|
||||
float t31 = t28+t29+t30+q1*t4-q1*t6;
|
||||
float t32 = q0*t4;
|
||||
float t33 = q0*t6;
|
||||
float t34 = q1*q2*q3*2.0f;
|
||||
float t35 = t32+t33+t34+q0*t3-q0*t5;
|
||||
float t8 = t7*t7;
|
||||
t8 = 1.0f/t8;
|
||||
float t11 = t2*t2;
|
||||
float t12 = t8*t11*4.0f;
|
||||
float t13 = t12+1.0f;
|
||||
float t14 = 1.0f/t13;
|
||||
|
||||
H_YAW[0] = t8*t14*t19*(-2.0f);
|
||||
H_YAW[1] = t8*t14*t27*(-2.0f);
|
||||
H_YAW[2] = t8*t14*t31*2.0f;
|
||||
H_YAW[3] = t8*t14*t35*2.0f;
|
||||
|
||||
// find largest difference as a fraction of the matlab value
|
||||
float max_diff_fraction = 0.0f;
|
||||
int max_row;
|
||||
float max_old, max_new;
|
||||
for (int row=0; row<4; row++) {
|
||||
float diff_fraction = fabsf(H_YAW_sympy[row] - H_YAW[row]) / fabsf(H_YAW[row]);
|
||||
if (diff_fraction > max_diff_fraction) {
|
||||
max_diff_fraction = diff_fraction;
|
||||
max_row = row;
|
||||
max_old = H_YAW[row];
|
||||
max_new = H_YAW_sympy[row];
|
||||
}
|
||||
}
|
||||
|
||||
if (max_diff_fraction > 1E-5f) {
|
||||
printf("Fail: 321 yaw option A Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
|
||||
} else {
|
||||
printf("Pass: 321 yaw option A Hfusion max diff fraction = %e\n",max_diff_fraction);
|
||||
}
|
||||
|
||||
// This second comparison for the 321 sequence option B equations that have a singularity when
|
||||
// yaw is at 0 and +-190 deg
|
||||
|
||||
// perform calculation using sympy generated equations
|
||||
const float SB0 = 2*q0;
|
||||
const float SB1 = 2*q1;
|
||||
const float SB2 = SB0*q3 + SB1*q2;
|
||||
const float SB3 = powf(SB2, -2);
|
||||
const float SB4 = powf(q0, 2) + powf(q1, 2) - powf(q2, 2) - powf(q3, 2);
|
||||
const float SB5 = 1.0F/(SB3*powf(SB4, 2) + 1);
|
||||
const float SB6 = 1.0F/SB2;
|
||||
const float SB7 = SB3*SB4;
|
||||
const float SB8 = 2*SB7;
|
||||
const float SB9 = 2*SB6;
|
||||
|
||||
H_YAW[0] = -SB5*(SB0*SB6 - SB8*q3);
|
||||
H_YAW[1] = -SB5*(SB1*SB6 - SB8*q2);
|
||||
H_YAW[2] = -SB5*(-SB1*SB7 - SB9*q2);
|
||||
H_YAW[3] = -SB5*(-SB0*SB7 - SB9*q3);
|
||||
|
||||
// save output and repeat calculation using legacy matlab generated code
|
||||
for (int row=0; row<4; row++) {
|
||||
H_YAW_sympy[row] = H_YAW[row];
|
||||
}
|
||||
|
||||
float t15 = t2*t2;
|
||||
t15 = 1.0f/t15;
|
||||
float t20 = t7*t7;
|
||||
float t21 = t15*t20*0.25f;
|
||||
float t22 = t21+1.0f;
|
||||
float t23 = 1.0f/t22;
|
||||
|
||||
H_YAW[0] = t15*t19*t23*(-0.5f);
|
||||
H_YAW[1] = t15*t23*t27*(-0.5f);
|
||||
H_YAW[2] = t15*t23*t31*0.5f;
|
||||
H_YAW[3] = t15*t23*t35*0.5f;
|
||||
|
||||
// find largest difference as a fraction of the matlab value
|
||||
max_diff_fraction = 0.0f;
|
||||
for (int row=0; row<4; row++) {
|
||||
float diff_fraction = fabsf(H_YAW_sympy[row] - H_YAW[row]) / fabsf(H_YAW[row]);
|
||||
if (diff_fraction > max_diff_fraction) {
|
||||
max_diff_fraction = diff_fraction;
|
||||
max_row = row;
|
||||
max_old = H_YAW[row];
|
||||
max_new = H_YAW_sympy[row];
|
||||
}
|
||||
}
|
||||
|
||||
if (max_diff_fraction > 1E-5f) {
|
||||
printf("Fail: 321 yaw option B Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
|
||||
} else {
|
||||
printf("Pass: 321 yaw option B Hfusion max diff fraction = %e\n",max_diff_fraction);
|
||||
}
|
||||
}
|
||||
|
||||
// calculate 312 yaw observation matrix using two computational paths to work around singularities
|
||||
// in calculation of the Jacobians.
|
||||
|
||||
{
|
||||
// This first comparison is for the 312 sequence option A equations that have a singularity when
|
||||
// yaw is at +- 90 deg
|
||||
|
||||
// perform calculation using sympy generated equations
|
||||
const float SA0 = 2*q3;
|
||||
const float SA1 = 2*q2;
|
||||
const float SA2 = SA0*q0 - SA1*q1;
|
||||
const float SA3 = powf(q0, 2) - powf(q1, 2) + powf(q2, 2) - powf(q3, 2);
|
||||
const float SA4 = powf(SA3, -2);
|
||||
const float SA5 = 1.0F/(powf(SA2, 2)*SA4 + 1);
|
||||
const float SA6 = 1.0F/SA3;
|
||||
const float SA7 = SA2*SA4;
|
||||
const float SA8 = 2*SA7;
|
||||
const float SA9 = 2*SA6;
|
||||
|
||||
H_YAW[0] = SA5*(SA0*SA6 - SA8*q0);
|
||||
H_YAW[1] = SA5*(-SA1*SA6 + SA8*q1);
|
||||
H_YAW[2] = SA5*(-SA1*SA7 - SA9*q1);
|
||||
H_YAW[3] = SA5*(SA0*SA7 + SA9*q0);
|
||||
|
||||
// save output and repeat calculation using legacy matlab generated code
|
||||
float H_YAW_sympy[4];
|
||||
for (int row=0; row<4; row++) {
|
||||
H_YAW_sympy[row] = H_YAW[row];
|
||||
}
|
||||
|
||||
// repeat calculation using matlab generated equations
|
||||
float t9 = q0*q3;
|
||||
float t10 = q1*q2;
|
||||
float t2 = t9-t10;
|
||||
float t3 = q0*q0;
|
||||
float t4 = q1*q1;
|
||||
float t5 = q2*q2;
|
||||
float t6 = q3*q3;
|
||||
float t7 = t3-t4+t5-t6;
|
||||
float t16 = q3*t3;
|
||||
float t17 = q3*t4;
|
||||
float t18 = t16+t17-q3*t5+q3*t6-q0*q1*q2*2.0f;
|
||||
float t23 = q2*t3;
|
||||
float t24 = q2*t4;
|
||||
float t25 = t23+t24+q2*t5-q2*t6-q0*q1*q3*2.0f;
|
||||
float t26 = q1*t5;
|
||||
float t27 = q1*t6;
|
||||
float t28 = t26+t27-q1*t3+q1*t4-q0*q2*q3*2.0f;
|
||||
float t29 = q0*t5;
|
||||
float t30 = q0*t6;
|
||||
float t31 = t29+t30+q0*t3-q0*t4-q1*q2*q3*2.0f;
|
||||
float t8 = t7*t7;
|
||||
float t15 = t2*t2;
|
||||
t8 = 1.0f/t8;
|
||||
float t11 = t2*t2;
|
||||
float t12 = t8*t11*4.0f;
|
||||
float t13 = t12+1.0f;
|
||||
float t14 = 1.0f/t13;
|
||||
|
||||
H_YAW[0] = t8*t14*t18*(-2.0f);
|
||||
H_YAW[1] = t8*t14*t25*(-2.0f);
|
||||
H_YAW[2] = t8*t14*t28*2.0f;
|
||||
H_YAW[3] = t8*t14*t31*2.0f;
|
||||
|
||||
// find largest difference as a fraction of the matlab value
|
||||
float max_diff_fraction = 0.0f;
|
||||
int max_row;
|
||||
float max_old, max_new;
|
||||
for (int row=0; row<4; row++) {
|
||||
float diff_fraction = fabsf(H_YAW_sympy[row] - H_YAW[row]) / fabsf(H_YAW[row]);
|
||||
if (diff_fraction > max_diff_fraction) {
|
||||
max_diff_fraction = diff_fraction;
|
||||
max_row = row;
|
||||
max_old = H_YAW[row];
|
||||
max_new = H_YAW_sympy[row];
|
||||
}
|
||||
}
|
||||
|
||||
if (max_diff_fraction > 1E-5f) {
|
||||
printf("Fail: 312 yaw option A Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
|
||||
} else {
|
||||
printf("Pass: 312 yaw option A Hfusion max diff fraction = %e\n",max_diff_fraction);
|
||||
}
|
||||
|
||||
// This second comparison for the 321 sequence option B equations that have a singularity when
|
||||
// yaw is at 0 and +-190 deg
|
||||
|
||||
// perform calculation using sympy generated equations
|
||||
const float SB0 = 2*q0;
|
||||
const float SB1 = 2*q1;
|
||||
const float SB2 = -SB0*q3 + SB1*q2;
|
||||
const float SB3 = powf(SB2, -2);
|
||||
const float SB4 = -powf(q0, 2) + powf(q1, 2) - powf(q2, 2) + powf(q3, 2);
|
||||
const float SB5 = 1.0F/(SB3*powf(SB4, 2) + 1);
|
||||
const float SB6 = 1.0F/SB2;
|
||||
const float SB7 = SB3*SB4;
|
||||
const float SB8 = 2*SB7;
|
||||
const float SB9 = 2*SB6;
|
||||
|
||||
H_YAW[0] = -SB5*(-SB0*SB6 + SB8*q3);
|
||||
H_YAW[1] = -SB5*(SB1*SB6 - SB8*q2);
|
||||
H_YAW[2] = -SB5*(-SB1*SB7 - SB9*q2);
|
||||
H_YAW[3] = -SB5*(SB0*SB7 + SB9*q3);
|
||||
|
||||
// save output and repeat calculation using legacy matlab generated code
|
||||
for (int row=0; row<4; row++) {
|
||||
H_YAW_sympy[row] = H_YAW[row];
|
||||
}
|
||||
|
||||
t15 = 1.0f/t15;
|
||||
float t19 = t7*t7;
|
||||
float t20 = t15*t19*0.25f;
|
||||
float t21 = t20+1.0f;
|
||||
float t22 = 1.0f/t21;
|
||||
|
||||
H_YAW[0] = t15*t18*t22*(-0.5f);
|
||||
H_YAW[1] = t15*t22*t25*(-0.5f);
|
||||
H_YAW[2] = t15*t22*t28*0.5f;
|
||||
H_YAW[3] = t15*t22*t31*0.5f;
|
||||
|
||||
// find largest difference as a fraction of the matlab value
|
||||
max_diff_fraction = 0.0f;
|
||||
for (int row=0; row<4; row++) {
|
||||
float diff_fraction = fabsf(H_YAW_sympy[row] - H_YAW[row]) / fabsf(H_YAW[row]);
|
||||
if (diff_fraction > max_diff_fraction) {
|
||||
max_diff_fraction = diff_fraction;
|
||||
max_row = row;
|
||||
max_old = H_YAW[row];
|
||||
max_new = H_YAW_sympy[row];
|
||||
}
|
||||
}
|
||||
|
||||
if (max_diff_fraction > 1E-5f) {
|
||||
printf("Fail: 312 yaw option B Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
|
||||
} else {
|
||||
printf("Pass: 312 yaw option B Hfusion max diff fraction = %e\n",max_diff_fraction);
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -48,14 +48,28 @@ def create_symmetric_cov_matrix():
|
||||
|
||||
return P
|
||||
|
||||
# generate equations for observation vector innovation variances
|
||||
def generate_observation_vector_innovation_variances(P,state,observation,variance,n_obs):
|
||||
H = observation.jacobian(state)
|
||||
innovation_variance = zeros(n_obs,1)
|
||||
for index in range(n_obs):
|
||||
H[index,:] = Matrix([observation[index]]).jacobian(state)
|
||||
innovation_variance[index] = H[index,:] * P * H[index,:].T + Matrix([variance])
|
||||
|
||||
IV_simple = cse(innovation_variance, symbols("IV0:1000"), optimizations='basic')
|
||||
|
||||
return IV_simple
|
||||
|
||||
# generate equations for observation Jacobian and Kalman gain
|
||||
def generate_observation_equations(P,state,observation,variance):
|
||||
def generate_observation_equations(P,state,observation,variance,varname="HK"):
|
||||
H = Matrix([observation]).jacobian(state)
|
||||
innov_var = H * P * H.T + Matrix([variance])
|
||||
assert(innov_var.shape[0] == 1)
|
||||
assert(innov_var.shape[1] == 1)
|
||||
K = P * H.T / innov_var[0,0]
|
||||
HK_simple = cse(Matrix([H.transpose(), K]), symbols("HK0:1000"), optimizations='basic')
|
||||
extension="0:1000"
|
||||
var_string = varname+extension
|
||||
HK_simple = cse(Matrix([H.transpose(), K]), symbols(var_string), optimizations='basic')
|
||||
|
||||
return HK_simple
|
||||
|
||||
@@ -86,18 +100,18 @@ def write_equations_to_file(equations,code_generator_id,n_obs):
|
||||
code_generator_id.print_string("Sub Expressions")
|
||||
code_generator_id.write_subexpressions(equations[0])
|
||||
code_generator_id.print_string("Observation Jacobians")
|
||||
code_generator_id.write_matrix(Matrix(equations[1][0][0:24]), "Hfusion")
|
||||
code_generator_id.write_matrix(Matrix(equations[1][0][0:24]), "Hfusion", False, ".at<", ">()")
|
||||
code_generator_id.print_string("Kalman gains")
|
||||
code_generator_id.write_matrix(Matrix(equations[1][0][24:]), "Kfusion")
|
||||
code_generator_id.write_matrix(Matrix(equations[1][0][24:]), "Kfusion", False, "(", ")")
|
||||
else:
|
||||
code_generator_id.print_string("Sub Expressions")
|
||||
code_generator_id.write_subexpressions(equations[0])
|
||||
for axis_index in range(n_obs):
|
||||
start_index = axis_index*48
|
||||
code_generator_id.print_string("Observation Jacobians - axis %i" % axis_index)
|
||||
code_generator_id.write_matrix(Matrix(equations[1][0][start_index:start_index+24]), "Hfusion")
|
||||
code_generator_id.write_matrix(Matrix(equations[1][0][start_index:start_index+24]), "Hfusion", False, ".at<", ">()")
|
||||
code_generator_id.print_string("Kalman gains - axis %i" % axis_index)
|
||||
code_generator_id.write_matrix(Matrix(equations[1][0][start_index+24:start_index+48]), "Kfusion")
|
||||
code_generator_id.write_matrix(Matrix(equations[1][0][start_index+24:start_index+48]), "Kfusion", False, "(", ")")
|
||||
|
||||
return
|
||||
|
||||
@@ -157,8 +171,8 @@ def body_frame_velocity_observation(P,state,R_to_body,vx,vy,vz):
|
||||
|
||||
vel_bf_code_generator.print_string("axis %i" % index)
|
||||
vel_bf_code_generator.write_subexpressions(equations[0])
|
||||
vel_bf_code_generator.write_matrix(Matrix(equations[1][0][0:24]), "H_VEL")
|
||||
vel_bf_code_generator.write_matrix(Matrix(equations[1][0][24:]), "Kfusion")
|
||||
vel_bf_code_generator.write_matrix(Matrix(equations[1][0][0:24]), "H_VEL", False, "(", ")")
|
||||
vel_bf_code_generator.write_matrix(Matrix(equations[1][0][24:]), "Kfusion", False, "(", ")")
|
||||
|
||||
vel_bf_code_generator.close()
|
||||
|
||||
@@ -267,11 +281,11 @@ def yaw_observation(P,state,R_to_earth):
|
||||
|
||||
yaw_code_generator.print_string("calculate 321 yaw observation matrix - option A")
|
||||
yaw_code_generator.write_subexpressions(H_YAW321_A_simple[0])
|
||||
yaw_code_generator.write_matrix(Matrix(H_YAW321_A_simple[1]).T, "H_YAW")
|
||||
yaw_code_generator.write_matrix(Matrix(H_YAW321_A_simple[1]).T, "H_YAW", False, ".at<", ">()")
|
||||
|
||||
yaw_code_generator.print_string("calculate 321 yaw observation matrix - option B")
|
||||
yaw_code_generator.write_subexpressions(H_YAW321_B_simple[0])
|
||||
yaw_code_generator.write_matrix(Matrix(H_YAW321_B_simple[1]).T, "H_YAW")
|
||||
yaw_code_generator.write_matrix(Matrix(H_YAW321_B_simple[1]).T, "H_YAW", False, ".at<", ">()")
|
||||
|
||||
# Derive observation Jacobian for fusion of 312 sequence yaw measurement
|
||||
# Calculate the yaw (first rotation) angle from an Euler 312 sequence
|
||||
@@ -286,16 +300,31 @@ def yaw_observation(P,state,R_to_earth):
|
||||
|
||||
yaw_code_generator.print_string("calculate 312 yaw observation matrix - option A")
|
||||
yaw_code_generator.write_subexpressions(H_YAW312_A_simple[0])
|
||||
yaw_code_generator.write_matrix(Matrix(H_YAW312_A_simple[1]).T, "H_YAW")
|
||||
yaw_code_generator.write_matrix(Matrix(H_YAW312_A_simple[1]).T, "H_YAW", False, ".at<", ">()")
|
||||
|
||||
yaw_code_generator.print_string("calculate 312 yaw observation matrix - option B")
|
||||
yaw_code_generator.write_subexpressions(H_YAW312_B_simple[0])
|
||||
yaw_code_generator.write_matrix(Matrix(H_YAW312_B_simple[1]).T, "H_YAW")
|
||||
yaw_code_generator.write_matrix(Matrix(H_YAW312_B_simple[1]).T, "H_YAW", False, ".at<", ">()")
|
||||
|
||||
yaw_code_generator.close()
|
||||
|
||||
return
|
||||
|
||||
# 3D magnetometer fusion
|
||||
def mag_observation_variance(P,state,R_to_body,i,ib):
|
||||
obs_var = symbols("R_MAG", real=True) # magnetometer measurement noise variance
|
||||
|
||||
m_mag = R_to_body * i + ib
|
||||
|
||||
# separate calculation of innovation variance equations for the y and z axes
|
||||
m_mag[0]=0
|
||||
innov_var_equations = generate_observation_vector_innovation_variances(P,state,m_mag,obs_var,3)
|
||||
mag_innov_var_code_generator = CodeGenerator("./generated/3Dmag_innov_var_generated.cpp")
|
||||
write_equations_to_file(innov_var_equations,mag_innov_var_code_generator,3)
|
||||
mag_innov_var_code_generator.close()
|
||||
|
||||
return
|
||||
|
||||
# 3D magnetometer fusion
|
||||
def mag_observation(P,state,R_to_body,i,ib):
|
||||
obs_var = symbols("R_MAG", real=True) # magnetometer measurement noise variance
|
||||
@@ -306,8 +335,17 @@ def mag_observation(P,state,R_to_body,i,ib):
|
||||
mag_code_generator = CodeGenerator("./generated/3Dmag_generated.cpp")
|
||||
|
||||
axes = [0,1,2]
|
||||
label="HK"
|
||||
for index in axes:
|
||||
equations = generate_observation_equations(P,state,m_mag[index],obs_var)
|
||||
if (index==0):
|
||||
label="HKX"
|
||||
elif (index==1):
|
||||
label="HKY"
|
||||
elif (index==2):
|
||||
label="HKZ"
|
||||
else:
|
||||
return
|
||||
equations = generate_observation_equations(P,state,m_mag[index],obs_var,varname=label)
|
||||
mag_code_generator.print_string("Axis %i equations" % index)
|
||||
write_equations_to_file(equations,mag_code_generator,1)
|
||||
|
||||
@@ -459,7 +497,7 @@ def generate_code():
|
||||
cov_code_generator = CodeGenerator("./generated/covariance_generated.cpp")
|
||||
cov_code_generator.print_string("Equations for covariance matrix prediction, without process noise!")
|
||||
cov_code_generator.write_subexpressions(P_new_simple[0])
|
||||
cov_code_generator.write_matrix(Matrix(P_new_simple[1]), "nextP", True)
|
||||
cov_code_generator.write_matrix(Matrix(P_new_simple[1]), "nextP", True, "(", ")")
|
||||
|
||||
cov_code_generator.close()
|
||||
|
||||
@@ -469,6 +507,7 @@ def generate_code():
|
||||
print('Generating gps heading observation code ...')
|
||||
gps_yaw_observation(P,state,R_to_body)
|
||||
print('Generating mag observation code ...')
|
||||
mag_observation_variance(P,state,R_to_body,i,ib)
|
||||
mag_observation(P,state,R_to_body,i,ib)
|
||||
print('Generating declination observation code ...')
|
||||
declination_observation(P,state,ix,iy)
|
||||
|
||||
+339
-339
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user