Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: solaESKF.cpp
- Revision:
- 44:7d82e63b6a86
- Parent:
- 43:cbc2c2d65131
- Child:
- 45:df4618814803
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/solaESKF.cpp Tue Oct 26 05:33:11 2021 +0000 @@ -0,0 +1,246 @@ +#include "solaESKF.hpp" +#include "Matrix.h" +#include "MatrixMath.h" +#include <cmath> + + +solaESKF::solaESKF() +{ + //nominal state + pihat(3,1); + vihat(3,1); + qhat(4,1); + accBias(3,1); + gyroBias(3,1); + gravity(3,1); + + + errState(18,1); + nState = errState.getRows(); + Phat(nState,nState); + Q(nState,nState); + + setDiag(Phat,0.1f); + setDiag(Q,0.01f); + setBlockDiag(Q,0.0f,1,3); + setBlockDiag(Q,0.00001f,16,18); + + +} + +void solaESKF::updateNominal(Matrix acc, Matrix gyro,float att_dt) +{ + Matrix gyrom = gyro - gyroBias; + Matrix accm = acc - accBias; + + Matrix qint(4,1); + qint << 1.0f << gyrom(1,1)*att_dt << gyrom(2,1)*att_dt << gyrom(3,1)*att_dt; + qhat = quatmultiply(qhat,qint); + float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat)); + qhat *= (1.0f/ qnorm); + + Matrix dcm(3,3); + computeDcm(dcm, qhat); + Matrix accned = dcm*acc+gravity; + vihat += accned*att_dt; + + pihat += vihat*att_dt+0.5f*accned*att_dt*att_dt; +} + +void solaESKF::updateErrState(Matrix acc,Matrix gyro,float att_dt) +{ + Matrix gyrom = gyro - gyroBias; + Matrix accm = acc - accBias; + + Matrix dcm(3,3); + computeDcm(dcm, qhat); + + Matrix Fx(nState,nState); + //position + 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; + + //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(4,16) = 1.0f*att_dt; + Fx(5,17) = 1.0f*att_dt; + Fx(6,18) = 1.0f*att_dt; + + //angulat error + 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; + + //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)+Q*att_dt*att_dt; +} + +/* +void solaESKF::updateGPSVelocity(float vi_x, float vi_y, float sinkRate,Vector3 acc, Vector3 accref) +{ + + Matrix H(3,nState); + H(1,10) = 1.0f; + H(2,11) = 1.0f; + H(3,12) = 1.0f; + + Matrix R(3,3); + R(1,1) = Rgps(1,1); + R(2,2) = Rgps(2,2); + R(3,3) = Rgps(3,3); + + Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); + Matrix z(3,1); + z << vi_x - vihat(1,1) << vi_y-vihat(2,1) << sinkRate - vihat(3,1); + //z << vi_x - vihat(1,1) << vi_y-vihat(2,1) << sinkRate - vihat(3,1); + Matrix corrVal = K * (z-H*errState); + errState = errState + corrVal; + Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(R)*MatrixMath::Transpose(K); +} +*/ +Matrix solaESKF::computeAngles() +{ + + Matrix euler(3,1); + euler(1,1) = atan2f(qhat(1,1)*qhat(2,1) + qhat(3,1)*qhat(4,1), 0.5f - qhat(2,1)*qhat(2,1) - qhat(3,1)*qhat(3,1)); + euler(2,1) = asinf(-2.0f * (qhat(2,1)*qhat(4,1) - qhat(1,1)*qhat(3,1))); + euler(3,1) = 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)); + return euler; +} + + +void solaESKF::fuseErr2Nominal() +{ + //position + pihat(1,1) += errState(1,1); + pihat(2,1) += errState(2,1); + pihat(3,1) += errState(3,1); + + //velocity + vihat(1,1) += errState(4,1); + vihat(2,1) += errState(5,1); + vihat(3,1) += errState(6,1); + + //angle error + Matrix qerr(4,1); + qerr << 1.0f << 0.5f*errState(1,7) << 0.5f*errState(8,1) << 0.5f*errState(9,1); + qhat = quatmultiply(qhat, qerr); + float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat)); + qhat *= (1.0f/ qnorm); + + //acc bias + accBias(1,1) += errState(10,1); + accBias(2,1) += errState(11,1); + accBias(3,1) += errState(12,1); + + //gyro bias + gyroBias(1,1) += errState(13,1); + gyroBias(2,1) += errState(14,1); + gyroBias(3,1) += errState(15,1); + + //gravity bias + gravity(1,1) += errState(16,1); + gravity(2,1) += errState(17,1); + gravity(3,1) += errState(18,1); + +} + +Matrix solaESKF::quatmultiply(Matrix p, Matrix q) +{ + + Matrix qout(4,1); + qout(1,1) = p(1,1)*q(1,1) - p(2,1)*q(2,1) - p(3,1)*q(3,1) - p(4,1)*q(4,1); + qout(2,1) = p(1,1)*q(2,1) + p(2,1)*q(1,1) + p(3,1)*q(4,1) - p(4,1)*q(3,1); + qout(3,1) = p(1,1)*q(3,1) - p(2,1)*q(4,1) + p(3,1)*q(1,1) + p(4,1)*q(2,1); + qout(4,1) = p(1,1)*q(4,1) + p(2,1)*q(3,1) - p(3,1)*q(2,1) + p(4,1)*q(1,1); + float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qout),qout)); + qout *= (1.0f/ qnorm); + return qout; +} + +void solaESKF::computeDcm(Matrix& dcm, Matrix quat) +{ + + dcm(1,1) = quat(1,1)*quat(1,1) + quat(2,1)*quat(2,1) - quat(3,1)*quat(3,1) - quat(4,1)*quat(4,1); + dcm(1,2) = 2.0f*(quat(2,1)*quat(3,1) - quat(1,1)*quat(4,1)); + dcm(1,3) = 2.0f*(quat(2,1)*quat(4,1) + quat(1,1)*quat(3,1)); + dcm(2,1) = 2.0f*(quat(2,1)*quat(3,1) + quat(1,1)*quat(4,1)); + dcm(2,2) = quat(1,1)*quat(1,1) - quat(2,1)*quat(2,1) + quat(3,1)*quat(3,1) - quat(4,1)*quat(4,1); + dcm(2,3) = 2.0f*(quat(3,1)*quat(4,1) - quat(1,1)*quat(2,1)); + dcm(3,1) = 2.0f*(quat(2,1)*quat(4,1) - quat(1,1)*quat(3,1)); + dcm(3,2) = 2.0f*(quat(3,1)*quat(4,1) + quat(1,1)*quat(2,1)); + dcm(3,3) = quat(1,1)*quat(1,1) - quat(2,1)*quat(2,1) - quat(3,1)*quat(3,1) + quat(4,1)*quat(4,1); + +} + +void solaESKF::setQhat(float ex,float ey,float ez) +{ + float cos_z_2 = cosf(0.5f*ez); + float cos_y_2 = cosf(0.5f*ey); + float cos_x_2 = cosf(0.5f*ex); + + float sin_z_2 = sinf(0.5f*ez); + float sin_y_2 = sinf(0.5f*ey); + float sin_x_2 = sinf(0.5f*ex); + + // and now compute quaternion + qhat(1,1) = cos_z_2*cos_y_2*cos_x_2 + sin_z_2*sin_y_2*sin_x_2; + qhat(2,1) = cos_z_2*cos_y_2*sin_x_2 - sin_z_2*sin_y_2*cos_x_2; + qhat(3,1) = cos_z_2*sin_y_2*cos_x_2 + sin_z_2*cos_y_2*sin_x_2; + qhat(4,1) = sin_z_2*cos_y_2*cos_x_2 - cos_z_2*sin_y_2*sin_x_2; +} + +void solaESKF::setGravity(float gx,float gy,float gz) +{ + gravity(1,1) = gx; + gravity(2,1) = gy; + gravity(3,1) = gz; +} + + + +void solaESKF::setDiag(Matrix& mat, float val){ + for (int i = 1; i < mat.getCols()+1; i++){ + mat(i,i) = val; + } +} + +void solaESKF::setBlockDiag(Matrix& mat, float val,int startIndex, int endIndez){ + for (int i = startIndex; i < endIndex+1; i++){ + mat(i,i) = val; + } +}