HAPSRG
Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: solaESKF.cpp
- Revision:
- 47:2467de40951f
- Parent:
- 46:15988dc41923
- Child:
- 48:06ed39e3376e
- Child:
- 50:dadad0567349
--- a/solaESKF.cpp Thu Oct 28 09:44:34 2021 +0000
+++ b/solaESKF.cpp Fri Oct 29 13:30:03 2021 +0000
@@ -24,20 +24,22 @@
nState = errState.getRows();
+
setBlockDiag(Phat,0.1f,1,3);//position
setBlockDiag(Phat,0.1f,4,6);//velocity
setBlockDiag(Phat,0.1f,7,9);//angle error
- setBlockDiag(Phat,0.01f,10,12);//acc bias
- setBlockDiag(Phat,0.01f,13,15);//gyro bias
- setBlockDiag(Phat,0.00001f,16,18);//gravity
- setBlockDiag(Q,0.0001f,4,6);//velocity
- setBlockDiag(Q,0.00001f,7,9);//angle error
+ setBlockDiag(Phat,0.1f,10,12);//acc bias
+ setBlockDiag(Phat,0.1f,13,15);//gyro bias
+ setBlockDiag(Phat,0.00000001f,16,18);//gravity
+ setBlockDiag(Q,0.00025f,4,6);//velocity
+ setBlockDiag(Q,0.005f/57.0,7,9);//angle error
setBlockDiag(Q,0.001f,10,12);//acc bias
setBlockDiag(Q,0.001f,13,15);//gyro bias //positionとgravityはQ項なし
}
+
void solaESKF::updateNominal(Matrix acc, Matrix gyro,float att_dt)
{
Matrix gyrom = gyro - gyroBias;
@@ -64,85 +66,43 @@
Matrix dcm(3,3);
computeDcm(dcm, qhat);
- Matrix a2v = -dcm*MatrixMath::Matrixcross(accm(1,1),accm(2,1),accm(3,1))*att_dt;
- Matrix a2v2 = 0.5f*a2v*att_dt;
+ Matrix a2v = -dcm*MatrixMath::Matrixcross(accm(1,1),accm(2,1),accm(3,1));
- Matrix Fx(nState,nState);
+ Matrix A(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;
+ A(1,4) = 1.0f;
+ A(2,5) = 1.0f;
+ A(3,6) = 1.0f;
+
+ //velocity
for (int i = 1; i < 4; i++){
for (int j = 1; j < 4; j++){
- Fx(i,j+6) = a2v2(i,j);
- Fx(i,j+9) = -0.5f*dcm(i,j)*att_dt*att_dt;
- }
- Fx(i,i+15) = 0.5f*att_dt*att_dt;
- }
-
-
- //velocity
- Fx(4,4) = 1.0f;
- Fx(5,5) = 1.0f;
- Fx(6,6) = 1.0f;
- 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(i+3,j+12) = -a2v2(i,j);
+ A(i+3,j+6) = a2v(i,j);
+ A(i+3,j+9) = -dcm(i,j);
+
}
}
- Fx(4,16) = 1.0f*att_dt;
- Fx(5,17) = 1.0f*att_dt;
- Fx(6,18) = 1.0f*att_dt;
+ A(4,16) = 1.0f;
+ A(5,17) = 1.0f;
+ A(6,18) = 1.0f;
//angulat error
- Fx(7,7) = 1.0f;
- Fx(8,8) = 1.0f;
- Fx(9,9) = 1.0f;
- 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;
+ A(7,8) = gyrom(3,1);
+ A(7,9) = -gyrom(2,1);
+ A(8,7) = -gyrom(3,1);
+ A(8,9) = gyrom(1,1);
+ A(9,7) = gyrom(2,1);
+ A(9,8) = -gyrom(1,1);
+ A(7,13) = -1.0f;
+ A(8,14) = -1.0f;
+ A(9,15) = -1.0f;
- //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;
+
+ Matrix Fx = 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 = Fx * errState;
- Matrix Qi(nState,nState);
- for (int i = 1; i < 4; i++){
- Qi(i,i) = 0.0f;
- }
- for (int i = 4; i < 10; i++){
- Qi(i,i) = Q(i,i)*att_dt*att_dt;
- }
- for (int i = 10; i < 16; i++){
- Qi(i,i) = Q(i,i)*att_dt;
- }
- for (int i = 16; i < 19; i++){
- Qi(i,i) = 0.0f;
- }
- Phat = Fx*Phat*MatrixMath::Transpose(Fx)+Qi;
+ Phat = Fx*Phat*MatrixMath::Transpose(Fx)+Qd;
}
void solaESKF::updateAccConstraints(Matrix acc,Matrix R)
@@ -165,11 +125,13 @@
H(2,11) = -1.0f;
H(3,12) = -1.0f;
- Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
- Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A));
-
+ //Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
+ //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A));
+ Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
Matrix z = -accm-tdcm*gravity;
errState = K * z;
+ //Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
+ //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
}
@@ -188,13 +150,16 @@
}
}
- Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
- Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(2))*MatrixMath::Transpose(A));
+ //Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
+ //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(2))*MatrixMath::Transpose(A));
+ Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
Matrix z3 = -dcm*gyrom;
Matrix z(2,1);
z << z3(1,1) << z3(2,1);
errState = K * z;
+ //Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
+ //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
}
@@ -207,11 +172,13 @@
H(2,5) = 1.0f;
H(3,6) = 1.0f;
- Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
- Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A));
+ //Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
+ //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A));
+ Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
Matrix z(3,1);
z << velgps(1,1) - vihat(1,1) << velgps(2,1)-vihat(2,1) << velgps(3,1) - vihat(3,1);
errState = K * z;
+ //Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
//Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
}
@@ -223,12 +190,13 @@
H(2,2) = 1.0f;
H(3,3) = 1.0f;
- Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
- Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+1000.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A));
- //Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
+ //Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
+ //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+1000.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A));
+ Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
Matrix z(3,1);
z << posgps(1,1) - pihat(1,1) << posgps(2,1)-pihat(2,1) << posgps(3,1) - pihat(3,1);
errState = K * z;
+ //Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
//Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
}
@@ -297,6 +265,10 @@
gravity(1,1) += errState(16,1);
gravity(2,1) += errState(17,1);
gravity(3,1) += errState(18,1);
+
+ for (int i = 1; i < 19; i++){
+ errState(i,1) = 0.0f;
+ }
}
@@ -345,13 +317,85 @@
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;
}
+Matrix solaESKF::getPihat()
+{
+ return pihat;
+}
+Matrix solaESKF::getVihat()
+{
+ return vihat;
+}
+Matrix solaESKF::getQhat()
+{
+ return qhat;
+}
+Matrix solaESKF::getAccBias()
+{
+ return accBias;
+}
+Matrix solaESKF::getGyroBias()
+{
+ return gyroBias;
+}
+Matrix solaESKF::getGravity()
+{
+ return gravity;
+}
+Matrix solaESKF::getErrState()
+{
+ return errState;
+}
+void solaESKF::setPhatPosition(float val)
+{
+ setBlockDiag(Phat,val,1,3);
+}
+void solaESKF::setPhatVelocity(float val)
+{
+ setBlockDiag(Phat,val,4,6);
+}
+void solaESKF::setPhatAngleError(float val)
+{
+ setBlockDiag(Phat,val,7,9);
+}
+void solaESKF::setPhatAccBias(float val)
+{
+ setBlockDiag(Phat,val,10,12);
+}
+void solaESKF::setPhatGyroBias(float val)
+{
+ setBlockDiag(Phat,val,13,15);
+}
+void solaESKF::setPhatGravity(float val)
+{
+ setBlockDiag(Phat,val,16,18);
+}
+
+
+void solaESKF::setQVelocity(float val)
+{
+ setBlockDiag(Q,val,4,6);
+}
+void solaESKF::setQAngleError(float val)
+{
+ setBlockDiag(Q,val,7,9);
+}
+void solaESKF::setQAccBias(float val)
+{
+ setBlockDiag(Q,val,10,12);
+}
+void solaESKF::setQGyroBias(float val)
+{
+ setBlockDiag(Q,val,13,15);
+}
void solaESKF::setDiag(Matrix& mat, float val){