solaESKF_Eigen - Eigen

HAPSRG » Code » solaESKF_Eigen
Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: ScErrStateEKF.cpp

Revision:: 20:37d3c3ee36e9
Parent:: 19:3fae66745363
Child:: 21:d6079def0473
--- a/ScErrStateEKF.cpp	Fri Jul 23 05:55:28 2021 +0000
+++ b/ScErrStateEKF.cpp	Sat Aug 07 06:54:50 2021 +0000
@@ -8,10 +8,10 @@
 using namespace std;
 
 ScErrStateEKF::ScErrStateEKF()
-    :qhat(4,1), errState(12,1), Phat(12,12), Q(12,12), Ra(5,5), Rm(3,3), Qab(5,5),accBias(0.0f,0.0f,0.0f),gyroBias(0.0f,0.0f,0.0f)
+    :qhat(4,1), errState(12,1), Phat(12,12), Q(12,12), Ra(3,3), Rm(3,3), Qab(3,3),Rgsc(2,2),Rvsc(3,3),accBias(0.0f,0.0f,0.0f),gyroBias(0.0f,0.0f,0.0f)
 { 
     qhat << 1.0f << 0.0f << 0.0f << 0.0f;
-    errState << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f;
+    errState << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f;
     
     Phat(1,1) = 0.1f;
     Phat(2,2) = 0.1f;
@@ -26,35 +26,40 @@
     Phat(11,11) = 0.1f;
     Phat(12,12) = 0.1f;
      
-    Q(1,1) = 0.000005f*0.25f;
-    Q(2,2) = 0.000005f*0.25f; 
-    Q(3,3) = 0.000005f*0.25f;
-    Q(4,4) = 0.0005f;
-    Q(5,5) = 0.0005f; 
-    Q(6,6) = 0.0005f;
-    Q(7,7) = 100.0f;
-    Q(8,8) = 100.0f;
-    Q(9,9) = 100.0f;
-    Q(10,10) = 100.0f;
-    Q(11,11) = 100.0f;
-    Q(12,12) = 100.0f;
+    Q(1,1) = 0.0000046f;
+    Q(2,2) = 0.0000046f; 
+    Q(3,3) = 0.0000046f;
+    Q(4,4) = 0.000013f;
+    Q(5,5) = 0.000013f; 
+    Q(6,6) = 0.000013f;
+    Q(7,7) = 0.0057f;
+    Q(8,8) = 0.0057f;
+    Q(9,9) = 0.0057f;
+    Q(10,10) = 0.01f;
+    Q(11,11) = 0.01f;
+    Q(12,12) = 0.01f;
     
-    Ra(1,1) = 0.000020f;
-    Ra(2,2) = 0.000020f;
-    Ra(3,3) = 0.000020f;
-    Ra(4,4) = 10.0f;
-    Ra(5,5) = 10.0f;
+    //加速度の観測
+    Ra(1,1) = 1.020f;
+    Ra(2,2) = 1.020f;
+    Ra(3,3) = 1.020f;
+    Qab(1,1) = 124.810f;
+    Qab(2,2) = 124.810f;
+    Qab(3,3) = 124.810f;
+    
+    //ジャイロバイアスに関する制約
+    Rgsc(1,1) = 263.980f;
+    Rgsc(2,2) = 263.980f;
+    
+    //速度に関する制約
+    Rvsc(1,1) = 5000.0f;
+    Rvsc(2,2) = 5000.0f;
+    Rvsc(3,3) = 5000.0f;
     
     Rm(1,1) = 5.0f;
     Rm(2,2) = 5.0f;
     Rm(3,3) = 5.0f;
     
-    Qab(1,1) = 100.7f;
-    Qab(2,2) = 100.7f;
-    Qab(3,3) = 100.7f;
-    Qab(4,4) = 0.0f;
-    Qab(5,5) = 0.0f;  
-    
     for(int i = 0; i<10;i++){
         histffunc[i] = 0.0f;
     }
@@ -73,12 +78,45 @@
       <<  0.5f*gyro.y << -0.5f*gyro.z << 0.0f        << 0.5f*gyro.x
       <<  0.5f*gyro.z <<  0.5f*gyro.y <<-0.5f*gyro.x << 0.0f ;
         
-    Matrix phi = MatrixMath::Eye(4) + A * att_dt + (0.5f*att_dt*att_dt) * A * A + (1.0f/6.0f*att_dt*att_dt*att_dt) * A * A * A;
+    Matrix phi = MatrixMath::Eye(4) + A * att_dt + (0.5f*att_dt*att_dt) * A * A;
     qhat = phi * qhat;
     float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
     qhat *= (1.0f/ qnorm);
 }
 
+void ScErrStateEKF::setQqerr(float val){
+    Q(1,1) = val;
+    Q(2,2) = val; 
+    Q(3,3) = val;
+}
+
+void ScErrStateEKF::setQgbias(float val){
+    Q(4,4) = val;
+    Q(5,5) = val; 
+    Q(6,6) = val;
+}
+void ScErrStateEKF::setQabias(float val){
+    Q(7,7) = val;
+    Q(8,8) = val; 
+    Q(9,9) = val;
+}
+
+void ScErrStateEKF::setQab(float val){
+    Qab(1,1) = val;
+    Qab(2,2) = val; 
+    Qab(3,3) = val;
+}
+
+void ScErrStateEKF::setRsoftconst(float Vgsc,float Vvsc){
+    Rgsc(1,1) = Vgsc;
+    Rgsc(2,2) = Vgsc;
+    Rvsc(1,1) = Vvsc;
+    Rvsc(2,2) = Vvsc;
+    Rvsc(3,3) = Vvsc;
+}
+
+
+
 void ScErrStateEKF::updateErrState(Vector3 gyro, float att_dt)
 {
     gyro -= gyroBias;
@@ -96,7 +134,7 @@
     A(11,8) = 1.0f;
     A(12,9) = 1.0f;
     
-    Matrix phi = MatrixMath::Eye(12) + A * att_dt + (0.5f*att_dt*att_dt) * A * A + (1.0f/6.0f*att_dt*att_dt*att_dt) * A * A * A;
+    Matrix phi = MatrixMath::Eye(12) + 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;
@@ -105,6 +143,34 @@
 void ScErrStateEKF::updateAccMeasures(Vector3 acc, Vector3 accref)
 {
     //acc -= accBias;
+    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(3,12);
+    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 K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Ra+Qab);
+    Matrix accmat(3,1);
+    accmat << acc.x << acc.y << acc.z;
+    Matrix gref(3,1);
+    gref << 0.0f << 0.0f << accref.z;
+    Matrix z = accmat-dcm*gref;
+    Matrix corrVal =  K * (z-H*errState);
+    errState = errState + corrVal;
+    Phat = (MatrixMath::Eye(12)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(12)-K*H)+K*(Ra+Qab)*MatrixMath::Transpose(K);
+}
+
+/*
+void ScErrStateEKF::updateScAccMeasures(Vector3 acc,Vector3 gyro, Vector3 accref)
+{
+    acc -= accBias;
     //acc = acc/acc.Norm();
     //accref = accref/accref.Norm();
     
@@ -121,29 +187,64 @@
     H(1,7) = 1.0f;
     H(2,8) = 1.0f;
     H(3,9) = 1.0f;
-    H(4,10) = 1.0f;
-    H(5,12) = 1.0f;
-    Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Ra+Qab);
+    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 K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Rasc+Qab);
     Matrix accmat(3,1);
     accmat << acc.x << acc.y << acc.z;
     Matrix gref(3,1);
     gref << 0.0f << 0.0f << accref.z;
     Matrix zacc = accmat-dcm*gref;
     Matrix z(5,1);
-    z << zacc(1,1) << zacc(2,1) << zacc(3,1) << 0.0f << 0.0f;
+    z << zacc(1,1) << zacc(2,1) << zacc(3,1) << -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(12)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(12)-K*H)+K*(Ra+Qab)*MatrixMath::Transpose(K);
+    Phat = (MatrixMath::Eye(9)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(9)-K*H)+K*(Rasc+Qab)*MatrixMath::Transpose(K);
 }
+*/
+
+void ScErrStateEKF::updateGyroBiasConstraints(Vector3 gyro)
+{
+    Matrix dcm(3,3);
+    computeDcm(dcm, qhat);
+    Matrix H(2,12);
+    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 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 corrVal =  K * (z-H*errState);
+    errState = errState + corrVal;
+    Phat = (MatrixMath::Eye(12)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(12)-K*H)+K*(Rgsc)*MatrixMath::Transpose(K);
+}
+
+
+void ScErrStateEKF::updateVelocityConstraints()
+{
+    Matrix H(3,12);
+    H(1,10) = 1.0f;
+    H(2,11) = 1.0f;
+    H(3,12) = 1.0f;
+    Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Rvsc);
+    Matrix corrVal =  K * (-H*errState);
+    errState = errState + corrVal;
+    Phat = (MatrixMath::Eye(12)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(12)-K*H)+K*(Rvsc)*MatrixMath::Transpose(K);
+}
+
 void ScErrStateEKF::updateStaticAccMeasures(Vector3 acc, Vector3 accref)
 {
-    //acc = acc/acc.Norm();
-    //accref = accref/accref.Norm();
-    
     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(3,9);
+    Matrix H(3,12);
     H(1,2) = -2.0f*gvec.z;
     H(1,3) =  2.0f*gvec.y;
     H(2,1) =  2.0f*gvec.z;
@@ -157,7 +258,7 @@
     gref << 0.0f << 0.0f << accref.z;
     Matrix z = accmat-dcm*gref;
     errState = errState + K * (z-H*errState);
-    Phat = (MatrixMath::Eye(9)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(9)-K*H)+K*(Ra*100.0f)*MatrixMath::Transpose(K);
+    Phat = (MatrixMath::Eye(12)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(12)-K*H)+K*(Ra*100.0f)*MatrixMath::Transpose(K);
 }
 
 
@@ -211,18 +312,20 @@
 
 void ScErrStateEKF::resetBias()
 {
+    /*
     gyroBias.x = gyroBias.x + errState(4,1)*1.0f;
     gyroBias.y = gyroBias.y + errState(5,1)*1.0f;
     gyroBias.z = gyroBias.z + errState(6,1)*1.0f;
     errState(4,1) = 0.0f;
     errState(5,1) = 0.0f;
     errState(6,1) = 0.0f;
-    //accBias.x = accBias.x + errState(7,1);
-    //accBias.y = accBias.y + errState(8,1);
-    //accBias.z = accBias.z + errState(9,1);
-    //errState(7,1) = 0.0f;
-    //errState(8,1) = 0.0f;
-    //errState(9,1) = 0.0f;
+    accBias.x = accBias.x + errState(7,1);
+    accBias.y = accBias.y + errState(8,1);
+    accBias.z = accBias.z + errState(9,1);
+    errState(7,1) = 0.0f;
+    errState(8,1) = 0.0f;
+    errState(9,1) = 0.0f;
+    */
 }
 
 void ScErrStateEKF::computeAngles(Vector3& rpy,Vector3 rpy_align)
Repository toolbox

Export to desktop IDE
Repository details

Type:	Library
Created:	06 Dec 2021
Imports:	1
Forks:	0
Commits:	89
Dependents:	2
Dependencies:	1
Followers:	25
This repository is Public (Unlisted).
Diff: ScErrStateEKF.cpp

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning
