Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: solaESKF.cpp
- Revision:
- 75:e2c825cdc511
- Parent:
- 74:f5fe7fecbd3c
- Child:
- 77:780ce6556041
diff -r f5fe7fecbd3c -r e2c825cdc511 solaESKF.cpp --- a/solaESKF.cpp Mon Nov 22 09:51:36 2021 +0000 +++ b/solaESKF.cpp Mon Nov 29 09:45:20 2021 +0000 @@ -59,51 +59,82 @@ { Matrix gyrom = gyro - gyroBias; Matrix accm = acc - accBias; - + Matrix dcm(3,3); computeDcm(dcm, qhat); - Matrix a2v = -dcm*MatrixMath::Matrixcross(accm(1,1),accm(2,1),accm(3,1)); - - Matrix A(nState,nState); + Matrix a2v = -dcm*MatrixMath::Matrixcross(accm(1,1),accm(2,1),accm(3,1))*att_dt; + Matrix a2v2 = 0.5f*a2v*att_dt; + + Matrix Fx(nState,nState); //position - A(1,4) = 1.0f; - A(2,5) = 1.0f; - A(3,6) = 1.0f; - - //velocity + Fx(1,1) = 1.0f; + Fx(2,2) = 1.0f; + Fx(3,3) = 1.0f; + Fx(1,4) = 1.0f*att_dt; + Fx(2,5) = 1.0f*att_dt; + Fx(3,6) = 1.0f*att_dt; for (int i = 1; i < 4; i++){ for (int j = 1; j < 4; j++){ - A(i+3,j+6) = a2v(i,j); - A(i+3,j+9) = -dcm(i,j); - A(i+3,j+15) = 1.0f; + Fx(i,j+6) = a2v2(i,j); + Fx(i,j+9) = -0.5f*dcm(i,j)*att_dt*att_dt; + } + Fx(i,i+15) = 0.5f*att_dt*att_dt; + } + + + //velocity + Fx(4,4) = 1.0f; + Fx(5,5) = 1.0f; + Fx(6,6) = 1.0f; + for (int i = 1; i < 4; i++){ + for (int j = 1; j < 4; j++){ + Fx(i+3,j+6) = a2v(i,j); + Fx(i+3,j+9) = -dcm(i,j)*att_dt; + Fx(i+3,j+12) = -a2v2(i,j); } } - - + Fx(4,16) = 1.0f*att_dt; + Fx(5,17) = 1.0f*att_dt; + Fx(6,18) = 1.0f*att_dt; + //angulat error - A(7,8) = gyrom(3,1); - A(7,9) = -gyrom(2,1); - A(8,7) = -gyrom(3,1); - A(8,9) = gyrom(1,1); - A(9,7) = gyrom(2,1); - A(9,8) = -gyrom(1,1); - A(7,13) = -1.0f; - A(8,14) = -1.0f; - A(9,15) = -1.0f; - + Fx(7,7) = 1.0f; + Fx(8,8) = 1.0f; + Fx(9,9) = 1.0f; + Fx(7,8) = gyrom(3,1)*att_dt; + Fx(7,9) = -gyrom(2,1)*att_dt; + Fx(8,7) = -gyrom(3,1)*att_dt; + Fx(8,9) = gyrom(1,1)*att_dt; + Fx(9,7) = gyrom(2,1)*att_dt; + Fx(9,8) = -gyrom(1,1)*att_dt; + Fx(7,13) = -1.0f*att_dt; + Fx(8,14) = -1.0f*att_dt; + Fx(9,15) = -1.0f*att_dt; - Matrix Fx = MatrixMath::Eye(nState) + A * att_dt;// + (0.5f*att_dt*att_dt) * A * A; + //acc bias + Fx(10,10) = 1.0f; + Fx(11,11) = 1.0f; + Fx(12,12) = 1.0f; + + //gyro bias + Fx(13,13) = 1.0f; + Fx(14,14) = 1.0f; + Fx(15,15) = 1.0f; + + //gravity bias + Fx(16,16) = 1.0f; + Fx(17,17) = 1.0f; + Fx(18,18) = 1.0f; + + errState = Fx * errState; + Phat = Fx*Phat*MatrixMath::Transpose(Fx); for (int i = 1; i < nState+1; i++){ if(i>3 && i<10){ - Q(i,i) *=att_dt; + Phat(i,i) += Q(i,i)*att_dt; }else if(i>9 && i<16){ - Q(i,i) *= att_dt*att_dt; - }else{ - Q(i,i) = 0.0f; + Phat(i,i) += Q(i,i)* att_dt*att_dt; } } - errState = Fx * errState; - Phat = Fx*Phat*MatrixMath::Transpose(Fx)+Q; } void solaESKF::updateAcc(Matrix acc,Matrix R) @@ -235,6 +266,24 @@ //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K); fuseErr2Nominal(); } +void solaESKF::updateGPSVelocity(Matrix velgps,Matrix R) +{ + Matrix H(3,nState); + H(1,4) = 1.0f; + H(2,5) = 1.0f; + H(3,6) = 1.0f; + + //Matrix A = H*Phat*MatrixMath::Transpose(H)+R; + //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+1000.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A)); + Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); + Matrix z(3,1); + z << velgps(1,1) - vihat(1,1) << velgps(2,1)-vihat(2,1)<< velgps(3,1)-vihat(3,1); + errState = K * z; + Phat = (MatrixMath::Eye(nState)-K*H)*Phat; + //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K); + //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K); + fuseErr2Nominal(); +} void solaESKF::updateGPS(Matrix posgps,float palt,Matrix velgps,Matrix R) { @@ -352,6 +401,16 @@ qhat(4,1) = sin_z_2*cos_y_2*cos_x_2 - cos_z_2*sin_y_2*sin_x_2; } +Matrix solaESKF::calcDynAcc(Matrix acc) +{ + Matrix accm = acc - accBias; + Matrix tdcm(3,3); + computeDcm(tdcm, qhat); + tdcm = MatrixMath::Transpose(tdcm); + + Matrix dynAcc = accm+tdcm*gravity; + return dynAcc; +} void solaESKF::setGravity(float gx,float gy,float gz)