HAPSRG
Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: ScErrStateEKF.cpp
- Revision:
- 21:d6079def0473
- Parent:
- 20:37d3c3ee36e9
- Child:
- 22:7d84b8bc20b4
--- a/ScErrStateEKF.cpp Sat Aug 07 06:54:50 2021 +0000
+++ b/ScErrStateEKF.cpp Tue Aug 10 08:08:25 2021 +0000
@@ -8,57 +8,28 @@
using namespace std;
ScErrStateEKF::ScErrStateEKF()
- :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(4,1),vihat(3,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)
{
+ nState = errState.getRows();
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;
- Phat(1,1) = 0.1f;
- Phat(2,2) = 0.1f;
- Phat(3,3) = 0.1f;
- Phat(4,4) = 0.1f;
- Phat(5,5) = 0.1f;
- Phat(6,6) = 0.1f;
- Phat(7,7) = 0.1f;
- Phat(8,8) = 0.1f;
- Phat(9,9) = 0.1f;
- Phat(10,10) = 0.1f;
- Phat(11,11) = 0.1f;
- Phat(12,12) = 0.1f;
-
- 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;
+ setDiag(Phat,0.1f);
+ setQqerr(0.000005f);
+ setQgbias(0.00001f);
+ setQabias(0.006f);
+ //setQv(0.000005f);
//加速度の観測
- 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;
+ setDiag(Ra,1.0f);
+ setDiag(Qab,120.0f);
//ジャイロバイアスに関する制約
- Rgsc(1,1) = 263.980f;
- Rgsc(2,2) = 263.980f;
+ setDiag(Rgsc,260.0f);
- //速度に関する制約
- Rvsc(1,1) = 5000.0f;
- Rvsc(2,2) = 5000.0f;
- Rvsc(3,3) = 5000.0f;
+ //機体軸速度に関する制約
+ //setDiag(Rvsc,100.0f);
- Rm(1,1) = 5.0f;
- Rm(2,2) = 5.0f;
- Rm(3,3) = 5.0f;
+ setDiag(Rm,5.0f);
for(int i = 0; i<10;i++){
histffunc[i] = 0.0f;
@@ -69,9 +40,10 @@
}
-void ScErrStateEKF::updateQhat(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
@@ -82,6 +54,10 @@
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::setQqerr(float val){
@@ -101,26 +77,27 @@
Q(9,9) = val;
}
+
void ScErrStateEKF::setQab(float val){
- Qab(1,1) = val;
- Qab(2,2) = val;
- Qab(3,3) = val;
+ setDiag(Qab,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;
+ setDiag(Rgsc,Vgsc);
+ setDiag(Rvsc,Vvsc);
}
-
+void ScErrStateEKF::setDiag(Matrix& mat, float val){
+ for (int i = 1; i < mat.getCols()+1; i++){
+ mat(i,i) = val;
+ }
+}
-void ScErrStateEKF::updateErrState(Vector3 gyro, float att_dt)
+void ScErrStateEKF::updateErrState(Vector3 gyro,Vector3 acc, float att_dt)
{
gyro -= gyroBias;
- Matrix A(12,12);
+ acc -= accBias;
+ Matrix A(nState,nState);
A(1,2) = gyro.z;
A(1,3) = -gyro.y;
A(2,1) = -gyro.z;
@@ -130,11 +107,19 @@
A(1,4) = -0.5f;
A(2,5) = -0.5f;
A(3,6) = -0.5f;
- A(10,7) = 1.0f;
- 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;
+ 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;
@@ -142,11 +127,11 @@
void ScErrStateEKF::updateAccMeasures(Vector3 acc, Vector3 accref)
{
- //acc -= accBias;
+ 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);
+ Matrix H(3,nState);
H(1,2) = -2.0f*gvec.z;
H(1,3) = 2.0f*gvec.y;
H(2,1) = 2.0f*gvec.z;
@@ -157,14 +142,10 @@
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 z = MatrixMath::Vector2mat(acc)-dcm*MatrixMath::Vector2mat(accref);
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(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Ra+Qab)*MatrixMath::Transpose(K);
}
/*
@@ -209,42 +190,112 @@
void ScErrStateEKF::updateGyroBiasConstraints(Vector3 gyro)
{
+ gyro -= gyroBias;
+
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 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 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;
+ 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);
+ Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-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 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 corrVal = K * (-H*errState);
+ Matrix z(2,1);
+ z << nomVb(1,1) << nomVb(3,1) ;
+ Matrix corrVal = K * (z-H*errState);
errState = errState + corrVal;
- Phat = (MatrixMath::Eye(12)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(12)-K*H)+K*(Rvsc)*MatrixMath::Transpose(K);
+ Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Rvsc)*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);
+ }
+ }
+ 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 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*(R)*MatrixMath::Transpose(K);
+}
+
+
void ScErrStateEKF::updateStaticAccMeasures(Vector3 acc, Vector3 accref)
{
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);
+ Matrix H(3,nState);
H(1,2) = -2.0f*gvec.z;
H(1,3) = 2.0f*gvec.y;
H(2,1) = 2.0f*gvec.z;
@@ -258,7 +309,7 @@
gref << 0.0f << 0.0f << accref.z;
Matrix z = accmat-dcm*gref;
errState = errState + K * (z-H*errState);
- Phat = (MatrixMath::Eye(12)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(12)-K*H)+K*(Ra*100.0f)*MatrixMath::Transpose(K);
+ Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Ra*100.0f)*MatrixMath::Transpose(K);
}
@@ -281,14 +332,14 @@
magrefmod(3,1) = magnedvec(3,1);
Matrix mvec = dcm*magrefmod ;
- Matrix H(3,9);
+ Matrix H(3,nState);
H(1,2) = -2.0f*magvec(3,1);
H(1,3) = 2.0f*magvec(2,1);
H(2,1) = 2.0f*magvec(3,1);
H(2,3) = -2.0f*magvec(1,1);
H(3,1) = -2.0f*magvec(2,1);
H(3,2) = 2.0f*magvec(1,1);
- Matrix Pm(9,9);
+ Matrix Pm(nState,nState);
for(int i = 1; i<4; i++){
for(int j = 1;j<4;j++){
Pm(i,j) = Phat(i,j);
@@ -297,7 +348,7 @@
Matrix r3(3,1);
r3 << dcm(1,3)<< dcm(2,3) << dcm(3,3);
Matrix kpart = r3*MatrixMath::Transpose(r3);
- Matrix Kmod(9,9);
+ Matrix Kmod(nState,nState);
for(int i = 1; i<4; i++){
for(int j = 1;j<4;j++){
Kmod(i,j) = kpart(i,j);
@@ -307,12 +358,11 @@
Matrix K = Kmod*(Pm*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Pm*MatrixMath::Transpose(H)+Rm);
Matrix z = magvec-mvec;
errState = errState + K * (z-H*errState);
- Phat = (MatrixMath::Eye(9)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(9)-K*H)+K*(Rm)*MatrixMath::Transpose(K);
+ Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Rm)*MatrixMath::Transpose(K);
}
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;
@@ -325,7 +375,6 @@
errState(7,1) = 0.0f;
errState(8,1) = 0.0f;
errState(9,1) = 0.0f;
- */
}
void ScErrStateEKF::computeAngles(Vector3& rpy,Vector3 rpy_align)
@@ -347,7 +396,18 @@
}
-void ScErrStateEKF::fuseErr2Qhat()
+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);
+
+}
+
+void ScErrStateEKF::fuseErr2Nominal()
{
Matrix qerr(4,1);
qerr << 1.0f << errState(1,1) << errState(2,1) << errState(3,1);
@@ -359,6 +419,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)
@@ -409,52 +476,6 @@
qhat(4,1) = sin_z_2*cos_y_2*cos_x_2 - cos_z_2*sin_y_2*sin_x_2;
}
-void ScErrStateEKF::triad(Vector3 fb, Vector3 fn, Vector3 mb, Vector3 mn){
- Matrix W1(3,1);
- W1 << fb.x << fb.y << fb.z;
- Matrix W2(3,1);
- W2 << mb.x << mb.y << mb.z;
-
- Matrix V1(3,1);
- V1 << fn.x << fn.y << fn.z;
- Matrix V2(3,1);
- V2 << mn.x << mn.y << mn.z;
-
-
- Matrix Ou2(3,1);
- Ou2 << W1( 2, 1 )*W2( 3, 1 )-W1( 3, 1 )*W2( 2, 1 ) << W1( 3, 1 )*W2( 1, 1 )-W1( 1, 1 )*W2( 3, 1 ) << W1( 1, 1 )*W2( 2, 1 )-W1( 2, 1 )*W2( 1, 1 );
- Ou2 *= 1.0f/sqrt(MatrixMath::dot(MatrixMath::Transpose(Ou2),Ou2));
- Matrix Ou3(3,1);
- Ou3 << W1( 2, 1 )*Ou2( 3, 1 )-W1( 3, 1 )*Ou2( 2, 1 ) << W1( 3, 1 )*Ou2( 1, 1 )-W1( 1, 1 )*Ou2( 3, 1 ) << W1( 1, 1 )*Ou2( 2, 1 )-W1( 2, 1 )*Ou2( 1, 1 );
- Ou3 *= 1.0f/sqrt(MatrixMath::dot(MatrixMath::Transpose(Ou3),Ou3));
- Matrix R2(3,1);
- R2 << V1( 2, 1 )*V2( 3, 1 )-V1( 3, 1 )*V2( 2, 1 ) << V1( 3, 1 )*V2( 1, 1 )-V1( 1, 1 )*V2( 3, 1 ) << V1( 1, 1 )*V2( 2, 1 )-V1( 2, 1 )*V2( 1, 1 );
- R2 *= 1.0f/sqrt(MatrixMath::dot(MatrixMath::Transpose(R2),R2));
- Matrix R3(3,1);
- R3 << V1( 2, 1 )*R2( 3, 1 )-V1( 3, 1 )*R2( 2, 1 ) << V1( 3, 1 )*R2( 1, 1 )-V1( 1, 1 )*R2( 3, 1 ) << V1( 1, 1 )*R2( 2, 1 )-V1( 2, 1 )*R2( 1, 1 );
- R3 *= 1.0f/sqrt(MatrixMath::dot(MatrixMath::Transpose(R3),R3));
-
- Matrix Mou(3,3);
- Mou << W1( 1, 1 ) << Ou2( 1, 1 ) << Ou3( 1, 1 )
- << W1( 2, 1 ) << Ou2( 2, 1 ) << Ou3( 2, 1 )
- << W1( 3, 1 ) << Ou2( 3, 1 ) << Ou3( 3, 1 );
- Matrix Mr(3,3);
- Mr << V1( 1, 1 ) << R2( 1, 1 ) << R3( 1, 1 )
- << V1( 2, 1 ) << R2( 2, 1 ) << R3( 2, 1 )
- << V1( 3, 1 ) << R2( 3, 1 ) << R3( 3, 1 );
-
- Matrix Cbn = Mr*MatrixMath::Transpose(Mou);
-
- float sqtrp1 = sqrt(1.0f+Cbn( 1, 1 )+Cbn( 2, 2 )+Cbn( 3, 3 ));
-
- qhat(1,1) = 0.5f*sqtrp1;
- qhat(2,1) = -(Cbn( 2, 3 )-Cbn( 3, 2 ))/2.0f/sqtrp1;
- qhat(3,1) = -(Cbn( 3, 1 )-Cbn( 1, 3 ))/2.0f/sqtrp1;
- qhat(4,1) = -(Cbn( 1, 2 )-Cbn( 2, 1 ))/2.0f/sqtrp1;
-
- float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
- qhat *= (1.0f/ qnorm);
-}
Vector3 ScErrStateEKF::calcDynAcc(Vector3 acc, Vector3 accref)