HAPSRG
Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: solaESKF.cpp
- Revision:
- 70:d12e46fdc2f0
- Parent:
- 68:264a7e0e4a29
- Child:
- 71:56c32be982b8
--- a/solaESKF.cpp Thu Nov 18 10:10:07 2021 +0000
+++ b/solaESKF.cpp Fri Nov 19 08:21:44 2021 +0000
@@ -3,7 +3,7 @@
solaESKF::solaESKF()
- :pihat(3,1),vihat(3,1),qhat(4,1),accBias(3,1),gyroBias(3,1),gravity(3,1),magBias(3,1),errState(20,1),Phat(20,20),Q(20,20)
+ :pihat(3,1),vihat(3,1),qhat(4,1),accBias(3,1),gyroBias(3,1),gravity(3,1),magBias(3,1),errState(16,1),Phat(16,16),Q(16,16)
{
pihat << 0.0f << 0.0f << 0.0f;
vihat << 0.0f << 0.0f << 0.0f;
@@ -30,14 +30,10 @@
setBlockDiag(Phat,0.1f,10,12);//acc bias
setBlockDiag(Phat,0.1f,13,15);//gyro bias
setBlockDiag(Phat,0.00000001f,16,16);//gravity
- setBlockDiag(Phat,0.001f,17,19);//magBias
- setBlockDiag(Phat,0.001f,20,20);//magRadius
setBlockDiag(Q,0.00025f,4,6);//velocity
setBlockDiag(Q,0.005f/57.0f,7,9);//angle error
setBlockDiag(Q,0.001f,10,12);//acc bias
- setBlockDiag(Q,0.001f,13,15);//gyro bias
- setBlockDiag(Q,0.001f,17,19);//magRadius
- setBlockDiag(Q,0.0001f,20,20);//magRadius //positionとgravityはQ項なし
+ setBlockDiag(Q,0.001f,13,15);//gyro bias//positionとgravityはQ項なし
}
@@ -217,19 +213,71 @@
fuseErr2Nominal();
}
+
+void solaESKF::updateHeading(Matrix mag,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;
+
+ 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;
+
+ Matrix Hpart = Hh*Hdq;
+ Matrix H(2,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 = 0.5f;
+ float gamma = 2.0f*q0q3q1q2/d0;
+ z << 1.0f/sqrt(a*a+1.0f)-1.0f/sqrt(gamma*gamma+1.0f) << a/sqrt(a*a+1.0f)-gamma/sqrt(gamma*gamma+1.0f);
+
+ Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
+ errState = K * z;
+ Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
+
+ fuseErr2Nominal();
+}
void solaESKF::updateImuConstraints(Matrix acc,Matrix mag,Matrix R)
{
Matrix accm = acc - accBias;
- Matrix magm = mag;
Matrix dcm(3,3);
computeDcm(dcm, qhat);
Matrix tdcm = MatrixMath::Transpose(dcm);
Matrix tdcm_g = tdcm*gravity;
Matrix rotgrav = MatrixMath::Matrixcross(tdcm_g(1,1),tdcm_g(2,1),tdcm_g(3,1));
- Matrix rotmag = -dcm*MatrixMath::Matrixcross(magm(1,1),magm(2,1),magm(3,1));
+ Matrix rotmag = -dcm*MatrixMath::Matrixcross(mag(1,1),mag(2,1),mag(3,1));
- Matrix H(5,nState);
- for (int i = 1; i < 4; i++){
+ Matrix H(6,nState);
+ for (int i = 1; i < 3; i++){
for (int j = 1; j < 4; j++){
H(i,j+6) = rotgrav(i,j);
}
@@ -240,21 +288,43 @@
H(2,11) = -1.0f;
H(3,12) = -1.0f;
- Matrix magned = dcm*magm;
+ Matrix magned = dcm*mag;
float hx = sqrt(magned(1,1)*magned(1,1)+magned(2,1)*magned(2,1));
+ float hz = sqrt(magned(3,1)*magned(3,1));
for(int j = 3; j < 4; j++){
H(4,j+6) = rotmag(1,j)-(rotmag(1,j)+rotmag(2,j))/hx;
//H(4,j+16) = -dcm(1,j)+(dcm(1,j)+dcm(2,j))/hx;
H(5,j+6) = rotmag(2,j);
//H(5,j+16) = -dcm(2,j);
+ H(6,j+6) = rotmag(3,j)-(rotmag(3,j))/hz;
+ }
+ Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
+ /*
+ Matrix r3(3,1);
+ r3 << tdcm(1,3)<< tdcm(2,3) << tdcm(3,3);
+ Matrix kpart = r3*MatrixMath::Transpose(r3);
+ Matrix Kmod(3,3);
+ for(int i = 1; i<4; i++){
+ for(int j = 1;j<4;j++){
+ Kmod(i,j) = K(i+6,j+3);
+ }
+ }
+ Kmod = kpart*Kmod;
+ */
+ for(int i = 1; i<nState+1; i++){
+ for(int j = 4;j<7;j++){
+ if(i>6 && i<10){
+ //K(i,j) = Kmod(i-6,j-3);
+ }else{
+ K(i,j) = 0.0f;
+ }
+ }
}
- Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
Matrix zacc = -accm-tdcm*gravity;
- Matrix zmag = dcm*magm;
- Matrix z(5,1);
- z << zacc(1,1) << zacc(2,1) << zacc(3,1) << -(zmag(1,1) - hx) << -zmag(2,1);
+ 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;
@@ -347,12 +417,7 @@
gravity(1,1) = 0.0f;
gravity(2,1) = 0.0f;
gravity(3,1) += errState(16,1);
-
- //Magnetic bias
- magBias(1,1) += errState(17,1);
- magBias(2,1) += errState(18,1);
- magBias(3,1) += errState(19,1);
- magRadius += errState(20,1);
+
for (int i = 1; i < nState+1; i++){