Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: solaESKF.cpp
- Revision:
- 48:06ed39e3376e
- Parent:
- 47:2467de40951f
- Child:
- 49:0a1976eb5420
diff -r 2467de40951f -r 06ed39e3376e solaESKF.cpp --- a/solaESKF.cpp Fri Oct 29 13:30:03 2021 +0000 +++ b/solaESKF.cpp Fri Oct 29 13:33:25 2021 +0000 @@ -63,46 +63,90 @@ { 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); + 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; + Matrix a2v = -dcm*MatrixMath::Matrixcross(acc(1,1),acc(2,1),acc(3,1))*att_dt; + 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); } } - A(4,16) = 1.0f; - A(5,17) = 1.0f; - A(6,18) = 1.0f; - + 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; + + //acc bias + Fx(10,10) = 1.0f; + Fx(11,11) = 1.0f; + Fx(12,12) = 1.0f; - Matrix Fx = MatrixMath::Eye(nState) + A * att_dt;// + (0.5f*att_dt*att_dt) * A * A; - Matrix Qd = Q * att_dt + 0.5f*A*Q * att_dt * att_dt+ 0.5f*Q*MatrixMath::Transpose(A) * att_dt * att_dt; - + //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)+Qd; + Phat = Fx*Phat*MatrixMath::Transpose(Fx)+Q*att_dt*att_dt; + Matrix Qi(nState,nState); + for (int i = 1; i < 4; i++){ + Qi(i,i) = 0.0f; + } + for (int i = 4; i < 10; i++){ + Qi(i,i) = Q(i,i)*att_dt*att_dt; + } + for (int i = 10; i < 16; i++){ + Qi(i,i) = Q(i,i)*att_dt; + } + for (int i = 16; i < 19; i++){ + Qi(i,i) = 0.0f; + } + Phat = Fx*Phat*MatrixMath::Transpose(Fx)+Qi; } void solaESKF::updateAccConstraints(Matrix acc,Matrix R)