HAPSRG
Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: ScErrStateEKF.cpp
- Revision:
- 22:7d84b8bc20b4
- Parent:
- 21:d6079def0473
- Child:
- 23:1509648c2318
--- a/ScErrStateEKF.cpp Tue Aug 10 08:08:25 2021 +0000
+++ b/ScErrStateEKF.cpp Tue Aug 10 08:29:19 2021 +0000
@@ -8,7 +8,7 @@
using namespace std;
ScErrStateEKF::ScErrStateEKF()
- :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)
+ :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)
{
nState = errState.getRows();
qhat << 1.0f << 0.0f << 0.0f << 0.0f;
@@ -24,7 +24,7 @@
setDiag(Qab,120.0f);
//ジャイロバイアスに関する制約
- setDiag(Rgsc,260.0f);
+ setDiag(Rgsc,500.0f);
//機体軸速度に関する制約
//setDiag(Rvsc,100.0f);
@@ -40,10 +40,9 @@
}
-void ScErrStateEKF::updateNominal(Vector3 gyro, Vector3 acc, Vector3 accref, float att_dt)
+void ScErrStateEKF::updateNominal(Vector3 gyro, 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
@@ -54,10 +53,6 @@
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){
@@ -84,7 +79,7 @@
void ScErrStateEKF::setRsoftconst(float Vgsc,float Vvsc){
setDiag(Rgsc,Vgsc);
- setDiag(Rvsc,Vvsc);
+ //setDiag(Rvsc,Vvsc);
}
void ScErrStateEKF::setDiag(Matrix& mat, float val){
@@ -93,10 +88,9 @@
}
}
-void ScErrStateEKF::updateErrState(Vector3 gyro,Vector3 acc, float att_dt)
+void ScErrStateEKF::updateErrState(Vector3 gyro, float att_dt)
{
gyro -= gyroBias;
- acc -= accBias;
Matrix A(nState,nState);
A(1,2) = gyro.z;
A(1,3) = -gyro.y;
@@ -108,17 +102,6 @@
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;
@@ -148,53 +131,13 @@
Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-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();
-
- 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,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;
- 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) << -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(9)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(9)-K*H)+K*(Rasc+Qab)*MatrixMath::Transpose(K);
-}
-*/
-
void ScErrStateEKF::updateGyroBiasConstraints(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 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++){
@@ -215,79 +158,6 @@
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::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)
@@ -396,16 +266,6 @@
}
-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()
{
@@ -419,13 +279,6 @@
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)
@@ -513,4 +366,143 @@
}
+/*
+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
+ << 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;
+ 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::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::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::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);
+
+}
+*/
+
+