dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2: remove old yaw fusion auto-code

This commit is contained in:
bresch 2022-10-18 16:24:13 +02:00 committed by Mathieu Bresciani
parent e33215b61c
commit cf9859965a
2 changed files with 0 additions and 390 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

315
src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_fusion_generated_compare.cpp
View File

@ -1,315 +0,0 @@
#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;
}

75
src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_generated.cpp
View File

@ -1,75 +0,0 @@
// calculate 321 yaw observation matrix - option A
const float SA0 = 2*q3;
const float SA1 = 2*q2;
const float SA2 = SA0*q0 + SA1*q1;
const float SA3 = (q0)*(q0) + (q1)*(q1) - (q2)*(q2) - (q3)*(q3);
const float SA4 = 1.0F/((SA3)*(SA3));
const float SA5 = 1.0F/((SA2)*(SA2)*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.at<0>() = SA5*(2*SA6*q3 - SA8*q0);
H_YAW.at<1>() = SA5*(2*SA6*q2 - SA8*q1);
H_YAW.at<2>() = SA5*(SA1*SA7 + SA9*q1);
H_YAW.at<3>() = SA5*(SA0*SA7 + SA9*q0);
// calculate 321 yaw observation matrix - option B
const float SB0 = 2*q0;
const float SB1 = 2*q1;
const float SB2 = SB0*q3 + SB1*q2;
const float SB3 = 1.0F/((SB2)*(SB2));
const float SB4 = (q0)*(q0) + (q1)*(q1) - (q2)*(q2) - (q3)*(q3);
const float SB5 = 1.0F/(SB3*(SB4)*(SB4) + 1);
const float SB6 = 1.0F/(SB2);
const float SB7 = SB3*SB4;
const float SB8 = 2*SB7;
const float SB9 = 2*SB6;
H_YAW.at<0>() = -SB5*(SB0*SB6 - SB8*q3);
H_YAW.at<1>() = -SB5*(SB1*SB6 - SB8*q2);
H_YAW.at<2>() = -SB5*(-SB1*SB7 - SB9*q2);
H_YAW.at<3>() = -SB5*(-SB0*SB7 - SB9*q3);
// calculate 312 yaw observation matrix - option A
const float SA0 = 2*q3;
const float SA1 = 2*q2;
const float SA2 = SA0*q0 - SA1*q1;
const float SA3 = (q0)*(q0) - (q1)*(q1) + (q2)*(q2) - (q3)*(q3);
const float SA4 = 1.0F/((SA3)*(SA3));
const float SA5 = 1.0F/((SA2)*(SA2)*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.at<0>() = SA5*(2*SA6*q3 - SA8*q0);
H_YAW.at<1>() = SA5*(-SA1*SA6 + SA8*q1);
H_YAW.at<2>() = SA5*(-SA1*SA7 - SA9*q1);
H_YAW.at<3>() = SA5*(SA0*SA7 + SA9*q0);
// calculate 312 yaw observation matrix - option B
const float SB0 = 2*q0;
const float SB1 = -SB0*q3 + 2*q1*q2;
const float SB2 = 1.0F/((SB1)*(SB1));
const float SB3 = -(q0)*(q0) + (q1)*(q1) - (q2)*(q2) + (q3)*(q3);
const float SB4 = 1.0F/(SB2*(SB3)*(SB3) + 1);
const float SB5 = 1.0F/(SB1);
const float SB6 = 2*q1;
const float SB7 = SB2*SB3;
const float SB8 = 2*SB5;
H_YAW.at<0>() = -SB4*(-SB0*SB5 + 2*SB2*SB3*q3);
H_YAW.at<1>() = -SB4*(SB5*SB6 - 2*SB7*q2);
H_YAW.at<2>() = -SB4*(-SB6*SB7 - SB8*q2);
H_YAW.at<3>() = -SB4*(SB0*SB7 + SB8*q3);