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