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;
+    }
+}