Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
solaESKF.cpp
- Committer:
- NaotoMorita
- Date:
- 2021-10-26
- Revision:
- 44:7d82e63b6a86
- Parent:
- ScErrStateEKF.cpp@ 43:cbc2c2d65131
- Child:
- 45:df4618814803
File content as of revision 44:7d82e63b6a86:
#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; } }