HAPSRG
Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: solaESKF.cpp
- Revision:
- 73:5770a0d470c0
- Parent:
- 72:8ebae608ae12
- Child:
- 74:f5fe7fecbd3c
--- a/solaESKF.cpp Fri Nov 19 14:24:09 2021 +0000
+++ b/solaESKF.cpp Sun Nov 21 08:24:40 2021 +0000
@@ -214,52 +214,80 @@
fuseErr2Nominal();
}
-void solaESKF::updateHeading(Matrix mag,Matrix R)
+void solaESKF::updateHeading(float a,Matrix R)
{
float q0 = qhat(1,1);
float q1 = qhat(2,1);
float q2 = qhat(3,1);
float q3 = qhat(4,1);
-
- float d0 = (-q3*q3-q2*q2+q1*q1+q0*q0);
- float q0q3q1q2 = (q0*q3+q1*q2);
- float h1lower = 2.0f*(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f)*sqrt(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f);
-
- float d1 = d0*sqrt(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f);
- float d2 = d0*d0*sqrt(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f);
- float d3 = d0*sqrt(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f)*(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f);
-
-
-
- Matrix Hh(2,4);
- Hh(1,1) = -(8.0f*q3*q0q3q1q2/d0/d0-16.0f*q0*q0q3q1q2*q0q3q1q2/d0/d0/d0)/h1lower;
- Hh(1,2) = -(8.0f*q2*q0q3q1q2/d0/d0-16.0f*q1*q0q3q1q2*q0q3q1q2/d0/d0/d0)/h1lower;
- Hh(1,3) = -(8.0f*q1*q0q3q1q2/d0/d0-16.0f*q2*q0q3q1q2*q0q3q1q2/d0/d0/d0)/h1lower;
- Hh(1,4) = -(8.0f*q0*q0q3q1q2/d0/d0-16.0f*q3*q0q3q1q2*q0q3q1q2/d0/d0/d0)/h1lower;
-
- Hh(2,1) = 2.0f*q3/d1-4.0f*q0*q0q3q1q2/d2-q0q3q1q2*(8.0f*q3*q0q3q1q2/d0/d0-16.0f*q0*q0q3q1q2*q0q3q1q2/d0/d0/d0)/d3;
- Hh(2,2) = 2.0f*q2/d1-4.0f*q1*q0q3q1q2/d2-q0q3q1q2*(8.0f*q2*q0q3q1q2/d0/d0-16.0f*q1*q0q3q1q2*q0q3q1q2/d0/d0/d0)/d3;
- Hh(2,3) = 2.0f*q1/d1+4.0f*q2*q0q3q1q2/d2-q0q3q1q2*(8.0f*q1*q0q3q1q2/d0/d0-16.0f*q2*q0q3q1q2*q0q3q1q2/d0/d0/d0)/d3;
- Hh(2,4) = 2.0f*q0/d1+4.0f*q3*q0q3q1q2/d2-q0q3q1q2*(8.0f*q0*q0q3q1q2/d0/d0-16.0f*q3*q0q3q1q2*q0q3q1q2/d0/d0/d0)/d3;
+
+ bool canUseA = false;
+ 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;
+ float SA4, SA5_inv;
+ if ((SA3*SA3) > 1e-6f) {
+ SA4 = 1.0F/(SA3*SA3);
+ SA5_inv = SA2*SA2*SA4 + 1;
+ canUseA = fabsf(SA5_inv) > 1e-6f;
+ }
+
+ bool canUseB = false;
+ const float SB0 = 2*q0;
+ const float SB1 = 2*q1;
+ const float SB2 = SB0*q3 + SB1*q2;
+ const float SB4 = q0*q0 + q1*q1 - q2*q2 - q3*q3;
+ float SB3, SB5_inv;
+ if ((SB2*SB2) > 1e-6f) {
+ SB3 = 1.0F/(SB2*SB2);
+ SB5_inv = SB3*SB4*SB4 + 1;
+ canUseB = fabsf(SB5_inv) > 1e-6f;
+ }
+
+ Matrix Hh(1,4);
+
+ if (canUseA && (!canUseB || fabsf(SA5_inv) >= fabsf(SB5_inv))) {
+ const float SA5 = 1.0F/SA5_inv;
+ const float SA6 = 1.0F/SA3;
+ const float SA7 = SA2*SA4;
+ const float SA8 = 2*SA7;
+ const float SA9 = 2*SA6;
+
+ Hh(1,1) = SA5*(SA0*SA6 - SA8*q0);
+ Hh(1,2) = SA5*(SA1*SA6 - SA8*q1);
+ Hh(1,3) = SA5*(SA1*SA7 + SA9*q1);
+ Hh(1,4) = SA5*(SA0*SA7 + SA9*q0);
+ } else if (canUseB && (!canUseA || fabsf(SB5_inv) > fabsf(SA5_inv))) {
+ const float SB5 = 1.0F/SB5_inv;
+ const float SB6 = 1.0F/SB2;
+ const float SB7 = SB3*SB4;
+ const float SB8 = 2*SB7;
+ const float SB9 = 2*SB6;
+
+ Hh(1,1) = -SB5*(SB0*SB6 - SB8*q3);
+ Hh(1,2) = -SB5*(SB1*SB6 - SB8*q2);
+ Hh(1,3) = -SB5*(-SB1*SB7 - SB9*q2);
+ Hh(1,4) = -SB5*(-SB0*SB7 - SB9*q3);
+ } else {
+ return;
+ }
Matrix Hdq(4,3);
Hdq << -0.5f*q1 << -0.5f*q2 << -0.5f*q3
<< 0.5f*q0 << -0.5f*q3 << 0.5f*q2
<< 0.5f*q3 << 0.5f*q0 << -0.5f*q1
- << -0.5f*q2 << 0.5f*q1 << 0.5f*q0;
+ << -0.5f*q2 << 0.5f*q1 << 0.5f*q0;
Matrix Hpart = Hh*Hdq;
- Matrix H(2,nState);
+ Matrix H(1,nState);
for(int j=1;j<4;j++){
H(1,j+6) = Hpart(1,j);
- H(2,j+6) = Hpart(2,j);
}
- Matrix z(2,1);
- float a = tan(180.0f*M_PI/180.0f);
- float gamma = 2.0f*q0q3q1q2/d0;
- z << -1.0f-1.0f/sqrt(gamma*gamma+1.0f) << 0.0f-gamma/sqrt(gamma*gamma+1.0f);
- twelite.printf("%f %f \r\n",z(1,1),z(2,1));
+ float psi = atan2f(qhat(2,1)*qhat(3,1) + qhat(1,1)*qhat(4,1), 0.5f - qhat(3,1)*qhat(3,1) - qhat(4,1)*qhat(4,1));
+ Matrix z(1,1);
+ z << atan2(sin(a-psi),cos(a-psi));
Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
errState = K * z;
Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
@@ -325,7 +353,6 @@
Matrix zacc = -accm-tdcm*gravity;
Matrix z(6,1);
z << zacc(1,1) << zacc(2,1) << zacc(3,1) << -(magned(1,1) - hx) << -magned(2,1) << -(magned(3,1) - hz);
- //twelite.printf("%f %f\r\n",hx,(zmag(1,1) - hx));
errState = K * z;
Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
@@ -595,6 +622,19 @@
pihat(2,1) = pi_y;
//pihat(3,1) = pi_z;
}
+
+float solaESKF::wrap_pi(float x) {
+ const float pi = 3.141592f;
+ const float range = pi + pi;
+ twelite.printf("1 : %f ",x);
+
+ if (x < -pi) {
+ x += range * ((-pi - x) / range + 1);
+ }
+
+ twelite.printf("2 : %f\r\n",-pi + fmodf( (x + pi),range));
+ return -pi + fmodf((x + pi),range);
+}
/*
void solaESKF::updateAccConstraints(Matrix acc,float palt,Matrix R)
{
@@ -805,4 +845,29 @@
fuseErr2Nominal();
}
+ float q0 = qhat(1,1);
+ float q1 = qhat(2,1);
+ float q2 = qhat(3,1);
+ float q3 = qhat(4,1);
+
+ float d0 = (-q3*q3-q2*q2+q1*q1+q0*q0);
+ float q0q3q1q2 = (q0*q3+q1*q2);
+ float h1lower = 2.0f*(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f)*sqrt(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f);
+
+ float d1 = d0*sqrt(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f);
+ float d2 = d0*d0*sqrt(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f);
+ float d3 = d0*sqrt(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f)*(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f);
+
+
+
+ Matrix Hh(2,4);
+ Hh(1,1) = -(8.0f*q3*q0q3q1q2/d0/d0-16.0f*q0*q0q3q1q2*q0q3q1q2/d0/d0/d0)/h1lower;
+ Hh(1,2) = -(8.0f*q2*q0q3q1q2/d0/d0-16.0f*q1*q0q3q1q2*q0q3q1q2/d0/d0/d0)/h1lower;
+ Hh(1,3) = -(8.0f*q1*q0q3q1q2/d0/d0-16.0f*q2*q0q3q1q2*q0q3q1q2/d0/d0/d0)/h1lower;
+ Hh(1,4) = -(8.0f*q0*q0q3q1q2/d0/d0-16.0f*q3*q0q3q1q2*q0q3q1q2/d0/d0/d0)/h1lower;
+
+ Hh(2,1) = 2.0f*q3/d1-4.0f*q0*q0q3q1q2/d2-q0q3q1q2*(8.0f*q3*q0q3q1q2/d0/d0-16.0f*q0*q0q3q1q2*q0q3q1q2/d0/d0/d0)/d3;
+ Hh(2,2) = 2.0f*q2/d1-4.0f*q1*q0q3q1q2/d2-q0q3q1q2*(8.0f*q2*q0q3q1q2/d0/d0-16.0f*q1*q0q3q1q2*q0q3q1q2/d0/d0/d0)/d3;
+ Hh(2,3) = 2.0f*q1/d1+4.0f*q2*q0q3q1q2/d2-q0q3q1q2*(8.0f*q1*q0q3q1q2/d0/d0-16.0f*q2*q0q3q1q2*q0q3q1q2/d0/d0/d0)/d3;
+ Hh(2,4) = 2.0f*q0/d1+4.0f*q3*q0q3q1q2/d2-q0q3q1q2*(8.0f*q0*q0q3q1q2/d0/d0-16.0f*q3*q0q3q1q2*q0q3q1q2/d0/d0/d0)/d3;
*/
\ No newline at end of file