Diff: ScErrStateEKF.cpp

Revision:

23:1509648c2318

Parent:

22:7d84b8bc20b4

Child:

25:07ac5c6cd61c

--- a/ScErrStateEKF.cpp	Tue Aug 10 08:29:19 2021 +0000
+++ b/ScErrStateEKF.cpp	Fri Aug 20 07:48:35 2021 +0000
@@ -8,7 +8,7 @@
 using namespace std;
 
 ScErrStateEKF::ScErrStateEKF()
-    :qhat(4,1), errState(9,1), Phat(9,9), Q(9,9), Ra(3,3), Rm(3,3), Qab(3,3),Rgsc(2,2),accBias(0.0f,0.0f,0.0f),gyroBias(0.0f,0.0f,0.0f)
+    :qhat(4,1),vihat(3,1), errState(12,1), Phat(12,12), Q(12,12), Ra(3,3), Rm(3,3), Qab(3,3),Rgsc(2,2),Rvsc(2,2),accBias(0.0f,0.0f,0.0f),gyroBias(0.0f,0.0f,0.0f)
 { 
     nState = errState.getRows();
     qhat << 1.0f << 0.0f << 0.0f << 0.0f;
@@ -16,18 +16,18 @@
     setDiag(Phat,0.1f);
     setQqerr(0.000005f);
     setQgbias(0.00001f);
-    setQabias(0.006f);
-    //setQv(0.000005f);
+    setQabias(1.0f);
+    setQv(0.000001f);
     
     //加速度の観測
-    setDiag(Ra,1.0f);
-    setDiag(Qab,120.0f);
+    setDiag(Ra,0.005f);
+    setDiag(Qab,100.0f);
     
     //ジャイロバイアスに関する制約
     setDiag(Rgsc,500.0f);
     
     //機体軸速度に関する制約
-    //setDiag(Rvsc,100.0f);
+    setDiag(Rvsc,100000.0f);
     
     setDiag(Rm,5.0f);
     
@@ -40,9 +40,10 @@
     
 }
 
-void ScErrStateEKF::updateNominal(Vector3 gyro, float att_dt)
+void ScErrStateEKF::updateNominal(Vector3 gyro, Vector3 acc, Vector3 accref, float att_dt)
 {
     gyro -= gyroBias;
+    acc -= accBias;
     Matrix A(4,4);
     A <<  0.0f        << -0.5f*gyro.x <<-0.5f*gyro.y <<-0.5f*gyro.z
       <<  0.5f*gyro.x <<  0.0f        << 0.5f*gyro.z <<-0.5f*gyro.y
@@ -53,8 +54,43 @@
     qhat = phi * qhat;
     float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
     qhat *= (1.0f/ qnorm);
+    
+    Matrix dcm(3,3);
+    computeDcm(dcm, qhat);
+    vihat += (MatrixMath::Transpose(dcm)*MatrixMath::Vector2mat(acc)-MatrixMath::Vector2mat(accref))*att_dt*9.8f;
 }
 
+void ScErrStateEKF::updateErrState(Vector3 gyro,Vector3 acc, float att_dt)
+{
+    gyro -= gyroBias;
+    acc -= accBias;
+    Matrix A(nState,nState);
+    A(1,2) =  gyro.z;
+    A(1,3) = -gyro.y;
+    A(2,1) = -gyro.z;
+    A(2,3) =  gyro.x;
+    A(3,1) =  gyro.y;
+    A(3,2) = -gyro.x;
+    A(1,4) =  -0.5f;
+    A(2,5) =  -0.5f;
+    A(3,6) =  -0.5f;
+    
+    Matrix dcm(3,3);
+    computeDcm(dcm, qhat);
+    Matrix qeterm = -2.0f*9.8f*MatrixMath::Transpose(dcm)*MatrixMath::Matrixcross(acc.x,acc.y,acc.z);
+    Matrix baterm = -9.8f*MatrixMath::Transpose(dcm);
+    for (int i = 1; i < 4; i++){
+        for (int j = 1; i < 4; i++){
+            A(i+9,j) = qeterm(i,j);
+            A(i+9,j+6) = baterm(i,j);
+        }
+    }
+    
+    Matrix phi = 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;
+    errState = phi * errState;
+    Phat = phi*Phat*MatrixMath::Transpose(phi)+Qd;
+}
 void ScErrStateEKF::setQqerr(float val){
     Q(1,1) = val;
     Q(2,2) = val; 
@@ -71,15 +107,20 @@
     Q(8,8) = val; 
     Q(9,9) = val;
 }
-
+void ScErrStateEKF::setQv(float val){
+    Q(10,10) = val;
+    Q(11,11) = val; 
+    Q(12,12) = val;
+}
 
 void ScErrStateEKF::setQab(float val){
     setDiag(Qab,val);
 }
 
+
 void ScErrStateEKF::setRsoftconst(float Vgsc,float Vvsc){
     setDiag(Rgsc,Vgsc);
-    //setDiag(Rvsc,Vvsc);
+    setDiag(Rvsc,Vvsc);
 }
 
 void ScErrStateEKF::setDiag(Matrix& mat, float val){
@@ -88,25 +129,6 @@
     }
 }
 
-void ScErrStateEKF::updateErrState(Vector3 gyro, float att_dt)
-{
-    gyro -= gyroBias;
-    Matrix A(nState,nState);
-    A(1,2) =  gyro.z;
-    A(1,3) = -gyro.y;
-    A(2,1) = -gyro.z;
-    A(2,3) =  gyro.x;
-    A(3,1) =  gyro.y;
-    A(3,2) = -gyro.x;
-    A(1,4) =  -0.5f;
-    A(2,5) =  -0.5f;
-    A(3,6) =  -0.5f;
-    
-    Matrix phi = 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;
-    errState = phi * errState;
-    Phat = phi*Phat*MatrixMath::Transpose(phi)+Qd;
-}
 
 void ScErrStateEKF::updateAccMeasures(Vector3 acc, Vector3 accref)
 {
@@ -137,8 +159,191 @@
     
     Matrix dcm(3,3);
     computeDcm(dcm, qhat);
-    Matrix qeterm = 2.0f*MatrixMath::Transpose(dcm)*MatrixMath::Matrixcross(gyro.x,gyro.y,gyro.z);
+    Matrix qeterm = -2.0f*MatrixMath::Transpose(dcm)*MatrixMath::Matrixcross(gyro.x,gyro.y,gyro.z);
+    Matrix H(2,nState);
+    for(int i = 1; i<3; i++){
+        for(int j = 1;j<4;j++){
+           H(i,j) = qeterm(i,j);
+        }
+    }
+    H(1,4) = 1.0f*(dcm(1,1));
+    H(1,5) = 1.0f*(dcm(2,1));
+    H(1,6) = 1.0f*(dcm(3,1));
+    H(2,4) = 1.0f*(dcm(1,2));
+    H(2,5) = 1.0f*(dcm(2,2));
+    H(2,6) = 1.0f*(dcm(3,2));
+    
+    Matrix Pm(nState,nState);
+    for(int i = 4; i<7; i++){
+        for(int j = 4;j<7;j++){
+            Pm(i,j) = Phat(i,j);
+        }
+    }
+    
+    Matrix K = (Pm*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Pm*MatrixMath::Transpose(H)+Rgsc);
+    Matrix z(2,1);
+    z <<dcm(1,1)*gyro.x+dcm(2,1)*gyro.y+dcm(3,1)*gyro.z<< dcm(1,2)*gyro.x+dcm(2,2)*gyro.y+dcm(3,2)*gyro.z;
+    Matrix corrVal =  K * (z-H*errState);
+    errState = errState + corrVal;
+    Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Rgsc)*MatrixMath::Transpose(K);
+}
+
+void ScErrStateEKF::updateVelocityConstraints()
+{
+    
+    Matrix dcm(3,3);
+    computeDcm(dcm, qhat);
     Matrix H(2,nState);
+    
+    Matrix nomVb = dcm*vihat;
+    Matrix qeterm = -2.0f*MatrixMath::Matrixcross(nomVb(1,1),nomVb(2,1),nomVb(3,1));
+    for(int j = 1;j<4;j++){
+        H(1,j) = -qeterm(1,j);
+        H(2,j) = -qeterm(3,j);
+        //H(3,j) = -qeterm(3,j);
+        H(1,9+j) = -dcm(1,j);
+        H(2,9+j) = -dcm(3,j);
+        //H(3,9+j) = -dcm(3,j);
+    }
+    
+    Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Rvsc);
+    Matrix z(2,1);
+    z << nomVb(1,1)  << nomVb(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*(Rvsc)*MatrixMath::Transpose(K);
+    
+}
+
+void ScErrStateEKF::updateMeasures(Vector3 gyro, Vector3 acc, Vector3 accref)
+{
+    acc -= accBias;
+    gyro -= gyroBias;
+    Matrix dcm(3,3);
+    computeDcm(dcm, qhat);
+    Vector3 gvec(dcm(1,3)*accref.z, dcm(2,3)*accref.z, dcm(3,3)*accref.z);
+    Matrix H(5,nState);
+    H(1,2) = -2.0f*gvec.z;
+    H(1,3) =  2.0f*gvec.y;
+    H(2,1) =  2.0f*gvec.z;
+    H(2,3) = -2.0f*gvec.x;
+    H(3,1) = -2.0f*gvec.y;
+    H(3,2) =  2.0f*gvec.x;
+    H(1,7) = 1.0f;
+    H(2,8) = 1.0f;
+    H(3,9) = 1.0f;
+    /*
+    Matrix qeterm = -2.0f*MatrixMath::Transpose(dcm)*MatrixMath::Matrixcross(gyro.x,gyro.y,gyro.z);
+    for(int j = 1;j<4;j++){
+        H(4,j) = qeterm(1,j);
+        H(5,j) = qeterm(2,j);
+    }
+    H(4,4) = 1.0f*(dcm(1,1));
+    H(4,5) = 1.0f*(dcm(2,1));
+    H(4,6) = 1.0f*(dcm(3,1));
+    H(5,4) = 1.0f*(dcm(1,2));
+    H(5,5) = 1.0f*(dcm(2,2));
+    H(5,6) = 1.0f*(dcm(3,2));
+    */
+    Matrix nomVb = dcm*vihat;
+    Matrix qeterm = 2.0f*MatrixMath::Matrixcross(nomVb(1,1),nomVb(2,1),nomVb(3,1));
+    for(int j = 1;j<4;j++){
+        H(4,j) = -qeterm(1,j);
+        H(5,j) = -qeterm(3,j);
+        //H(3,j) = -qeterm(3,j);
+        H(4,9+j) = -dcm(1,j);
+        H(5,9+j) = -dcm(3,j);
+        //H(3,9+j) = -dcm(3,j);
+    }
+    
+    Matrix R(5,5);
+    R(1,1) = Ra(1,1)+Qab(1,1);
+    R(2,2) = Ra(2,2)+Qab(2,2);
+    R(3,3) = Ra(3,3)+Qab(3,3);
+    //R(4,4) = Rgsc(1,1);
+    //R(5,5) = Rgsc(2,2);
+    R(4,4) = Rvsc(1,1);
+    R(5,5) = Rvsc(2,2);
+
+    Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
+    Matrix zacc = MatrixMath::Vector2mat(acc)-dcm*MatrixMath::Vector2mat(accref);
+    Matrix z(5,1);
+    z << zacc(1,1)<< zacc(2,1)<< zacc(3,1)  << nomVb(1,1)  << nomVb(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);
+}
+
+
+void ScErrStateEKF::updateStaticMeasures(Vector3 gyro,Vector3 acc, Vector3 accref)
+{
+    acc -= accBias;
+    gyro -= gyroBias;
+    Matrix dcm(3,3);
+    computeDcm(dcm, qhat);
+    Vector3 gvec(dcm(1,3)*accref.z, dcm(2,3)*accref.z, dcm(3,3)*accref.z);
+    Matrix H(5,nState);
+    H(1,2) = -2.0f*gvec.z;
+    H(1,3) =  2.0f*gvec.y;
+    H(2,1) =  2.0f*gvec.z;
+    H(2,3) = -2.0f*gvec.x;
+    H(3,1) = -2.0f*gvec.y;
+    H(3,2) =  2.0f*gvec.x;
+    H(1,7) = 1.0f;
+    H(2,8) = 1.0f;
+    H(3,9) = 1.0f;
+    /*
+    Matrix qeterm = -2.0f*MatrixMath::Transpose(dcm)*MatrixMath::Matrixcross(gyro.x,gyro.y,gyro.z);
+    for(int j = 1;j<4;j++){
+        H(4,j) = qeterm(1,j);
+        H(5,j) = qeterm(2,j);
+    }
+    H(4,4) = 1.0f*(dcm(1,1));
+    H(4,5) = 1.0f*(dcm(2,1));
+    H(4,6) = 1.0f*(dcm(3,1));
+    H(5,4) = 1.0f*(dcm(1,2));
+    H(5,5) = 1.0f*(dcm(2,2));
+    H(5,6) = 1.0f*(dcm(3,2));
+    */
+    Matrix nomVb = dcm*vihat;
+    Matrix qeterm = 2.0f*MatrixMath::Matrixcross(nomVb(1,1),nomVb(2,1),nomVb(3,1));
+    for(int j = 1;j<4;j++){
+        H(4,j) = -qeterm(1,j);
+        H(5,j) = -qeterm(3,j);
+        //H(3,j) = -qeterm(3,j);
+        H(4,9+j) = -dcm(1,j);
+        H(5,9+j) = -dcm(3,j);
+        //H(3,9+j) = -dcm(3,j);
+    }
+    
+    Matrix R(5,5);
+    R(1,1) = Ra(1,1)*100.0f;
+    R(2,2) = Ra(2,2)*100.0f;
+    R(3,3) = Ra(3,3)*100.0f;
+    //R(4,4) = Rgsc(1,1);
+    //R(5,5) = Rgsc(2,2);
+    R(4,4) = Rvsc(1,1);
+    R(5,5) = Rvsc(2,2);
+    
+    Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
+    Matrix zacc = MatrixMath::Vector2mat(acc)-dcm*MatrixMath::Vector2mat(accref);
+    Matrix z(5,1);
+    z << zacc(1,1)<< zacc(2,1)<< zacc(3,1) << nomVb(1,1)  << nomVb(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);
+}
+
+void ScErrStateEKF::updateConstraints(Vector3 gyro)
+{
+    
+    gyro -=gyroBias;
+    
+    Matrix dcm(3,3);
+    computeDcm(dcm, qhat);
+    
+    Matrix qeterm = -2.0f*MatrixMath::Transpose(dcm)*MatrixMath::Matrixcross(gyro.x,gyro.y,gyro.z);
+    Matrix H(4,nState);
     for(int i = 1; i<3; i++){
         for(int j = 1;j<4;j++){
             H(i,j) = qeterm(i,j);
@@ -150,14 +355,32 @@
     H(2,4) = 1.0f*(dcm(1,2));
     H(2,5) = 1.0f*(dcm(2,2));
     H(2,6) = 1.0f*(dcm(3,2));
-    Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Rgsc);
-    Matrix z(2,1);
-    z <<dcm(1,1)*gyro.x+dcm(2,1)*gyro.y+dcm(3,1)*gyro.z<< dcm(1,2)*gyro.x+dcm(2,2)*gyro.y+dcm(3,2)*gyro.z;
+    
+    Matrix nomVb = dcm*vihat;
+    qeterm = -2.0f*MatrixMath::Matrixcross(nomVb(1,1),nomVb(2,1),nomVb(3,1));
+    for(int j = 1;j<4;j++){
+        H(3,j) = -qeterm(1,j);
+        H(4,j) = -qeterm(3,j);
+        //H(3,j) = -qeterm(3,j);
+        H(3,9+j) = -dcm(1,j);
+        H(4,9+j) = -dcm(3,j);
+        //H(3,9+j) = -dcm(3,j);
+    }
+    
+    
+    Matrix R(4,4);
+    R(1,1) = Rgsc(1,1);
+    R(2,2) = Rgsc(2,1);
+    R(3,3) = Rvsc(1,1);
+    R(4,4) = Rvsc(2,1);
+    //R(5,5) = Rvsc(3,1);
+    Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
+    Matrix z(4,1);
+    z <<dcm(1,1)*gyro.x+dcm(2,1)*gyro.y+dcm(3,1)*gyro.z<< dcm(1,2)*gyro.x+dcm(2,2)*gyro.y+dcm(3,2)*gyro.z << nomVb(1,1)  << nomVb(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*(Rgsc)*MatrixMath::Transpose(K);
-}
-
+    Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(R)*MatrixMath::Transpose(K);
+}  
 
 
 void ScErrStateEKF::updateStaticAccMeasures(Vector3 acc, Vector3 accref)
@@ -182,8 +405,6 @@
     Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Ra*100.0f)*MatrixMath::Transpose(K);
 }
 
-
-
 void ScErrStateEKF::updateMagMeasures(Vector3 mag)
 {
     //mag = mag/mag.Norm();
@@ -279,6 +500,13 @@
     errState(3,1) = 0.0f;
     float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
     qhat *= (1.0f/ qnorm);
+    
+    vihat(1,1) += errState(10,1);
+    vihat(2,1) += errState(11,1);
+    vihat(3,1) += errState(12,1);
+    errState(10,1) = 0.0f;
+    errState(11,1) = 0.0f;
+    errState(12,1) = 0.0f;
 }
 
 Matrix ScErrStateEKF::quatmultiply(Matrix q, Matrix r)
@@ -329,6 +557,17 @@
         qhat(4,1) = sin_z_2*cos_y_2*cos_x_2 - cos_z_2*sin_y_2*sin_x_2;    
 }
 
+void ScErrStateEKF::computeVb(Vector3& vb)
+{
+    Matrix dcm(3,3);
+    computeDcm(dcm, qhat);
+    Matrix vbmat = dcm*vihat;
+    vb.x = vbmat(1,1);
+    vb.y = vbmat(2,1);
+    vb.z = vbmat(3,1);
+    
+}
+
 
 
 Vector3 ScErrStateEKF::calcDynAcc(Vector3 acc, Vector3 accref)