HAPSRG
Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: solaESKF.cpp
- Revision:
- 46:15988dc41923
- Parent:
- 45:df4618814803
- Child:
- 47:2467de40951f
--- a/solaESKF.cpp Tue Oct 26 05:36:11 2021 +0000
+++ b/solaESKF.cpp Thu Oct 28 09:44:34 2021 +0000
@@ -5,25 +5,35 @@
solaESKF::solaESKF()
+ :pihat(3,1),vihat(3,1),qhat(4,1),accBias(3,1),gyroBias(3,1),gravity(3,1),errState(18,1),Phat(18,18),Q(18,18)
{
- //nominal state
- pihat(3,1);
- vihat(3,1);
- qhat(4,1);
- accBias(3,1);
- gyroBias(3,1);
- gravity(3,1);
+ pihat << 0.0f << 0.0f << 0.0f;
+ vihat << 0.0f << 0.0f << 0.0f;
+ qhat << 1.0f << 0.0f << 0.0f << 0.0f;
+ accBias << 0.0f << 0.0f << 0.0f;
+ gyroBias << 0.0f << 0.0f << 0.0f;
+ gravity << 0.0f << 0.0f << 9.8f;
+
+ for (int i = 1; i < 19; i++){
+ errState(i,1) = 0.0f;
+ for (int j = 1; j < 19; j++){
+ Phat(i,j) = 0.0f;
+ Q(i,j) = 0.0f;
+ }
+ }
- errState(18,1);
nState = errState.getRows();
- Phat(nState,nState);
- Q(nState,nState);
-
- setDiag(Phat,0.1f);
- setDiag(Q,0.01f);
- setBlockDiag(Q,0.0f,1,3);
- setBlockDiag(Q,0.00001f,16,18);
+ 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(Q,0.001f,10,12);//acc bias
+ setBlockDiag(Q,0.001f,13,15);//gyro bias //positionとgravityはQ項なし
}
@@ -34,14 +44,14 @@
Matrix accm = acc - accBias;
Matrix qint(4,1);
- qint << 1.0f << gyrom(1,1)*att_dt << gyrom(2,1)*att_dt << gyrom(3,1)*att_dt;
+ qint << 1.0f << 0.5f*gyrom(1,1)*att_dt << 0.5f*gyrom(2,1)*att_dt << 0.5f*gyrom(3,1)*att_dt;
qhat = quatmultiply(qhat,qint);
float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
qhat *= (1.0f/ qnorm);
Matrix dcm(3,3);
computeDcm(dcm, qhat);
- Matrix accned = dcm*acc+gravity;
+ Matrix accned = dcm*accm+gravity;
vihat += accned*att_dt;
pihat += vihat*att_dt+0.5f*accned*att_dt*att_dt;
@@ -54,6 +64,8 @@
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 Fx(nState,nState);
//position
@@ -63,16 +75,24 @@
Fx(1,4) = 1.0f*att_dt;
Fx(2,5) = 1.0f*att_dt;
Fx(3,6) = 1.0f*att_dt;
+ 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;
- Matrix a2v = -dcm*MatrixMath::Matrixcross(acc(1,1),acc(2,1),acc(3,1))*att_dt;
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);
}
}
Fx(4,16) = 1.0f*att_dt;
@@ -80,6 +100,9 @@
Fx(6,18) = 1.0f*att_dt;
//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;
@@ -106,32 +129,130 @@
Fx(18,18) = 1.0f;
errState = Fx * errState;
- Phat = Fx*Phat*MatrixMath::Transpose(Fx)+Q*att_dt*att_dt;
+ 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;
}
-/*
-void solaESKF::updateGPSVelocity(float vi_x, float vi_y, float sinkRate,Vector3 acc, Vector3 accref)
+void solaESKF::updateAccConstraints(Matrix acc,Matrix R)
+{
+ Matrix accm = acc - accBias;
+ Matrix tdcm(3,3);
+ computeDcm(tdcm, qhat);
+ tdcm = MatrixMath::Transpose(tdcm);
+ Matrix tdcm_g = tdcm*gravity;
+ Matrix a2v = MatrixMath::Matrixcross(tdcm_g(1,1),tdcm_g(2,1),tdcm_g(3,1));
+
+ Matrix H(3,nState);
+ for (int i = 1; i < 4; i++){
+ for (int j = 1; j < 4; j++){
+ H(i,j+6) = a2v(i,j);
+ H(i,j+15) = tdcm(i,j);
+ }
+ }
+ H(1,10) = -1.0f;
+ 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 z = -accm-tdcm*gravity;
+ errState = K * z;
+ Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
+}
+
+void solaESKF::updateGyroConstraints(Matrix gyro,Matrix R)
+{
+ Matrix gyrom = gyro - gyroBias;
+ Matrix dcm(3,3);
+ computeDcm(dcm, qhat);
+ Matrix a2v = -dcm*MatrixMath::Matrixcross(gyrom(1,1),gyrom(2,1),gyrom(3,1));
+
+ Matrix H(2,nState);
+ for (int i = 1; i < 3; i++){
+ for (int j = 1; j < 4; j++){
+ H(i,j+6) = a2v(i,j);
+ H(i,j+12) = -dcm(i,j);
+ }
+ }
+
+ 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 z3 = -dcm*gyrom;
+ Matrix z(2,1);
+ z << z3(1,1) << z3(2,1);
+ errState = K * z;
+ Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
+}
+
+
+void solaESKF::updateGPSVelocity(Matrix velgps,Matrix R)
{
Matrix H(3,nState);
- H(1,10) = 1.0f;
- H(2,11) = 1.0f;
- H(3,12) = 1.0f;
+ H(1,4) = 1.0f;
+ 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 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 = 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);
+}
- Matrix R(3,3);
- R(1,1) = Rgps(1,1);
- R(2,2) = Rgps(2,2);
- R(3,3) = Rgps(3,3);
+void solaESKF::updateGPSPosition(Matrix posgps,Matrix R)
+{
+ Matrix H(3,nState);
+ H(1,1) = 1.0f;
+ H(2,2) = 1.0f;
+ H(3,3) = 1.0f;
- 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 << vi_x - vihat(1,1) << vi_y-vihat(2,1) << sinkRate - vihat(3,1);
- //z << vi_x - vihat(1,1) << vi_y-vihat(2,1) << sinkRate - vihat(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);
+ z << posgps(1,1) - pihat(1,1) << posgps(2,1)-pihat(2,1) << posgps(3,1) - pihat(3,1);
+ errState = K * z;
+ //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);
}
-*/
+
+void solaESKF::updateGPS(Matrix posgps,Matrix velgps,Matrix R)
+{
+
+ Matrix H(6,nState);
+ H(1,1) = 1.0f;
+ H(2,2) = 1.0f;
+ H(3,3) = 1.0f;
+ H(4,4) = 1.0f;
+ H(5,5) = 1.0f;
+ H(6,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(6))*MatrixMath::Transpose(A));
+ Matrix z(6,1);
+ z << posgps(1,1) - pihat(1,1) << posgps(2,1)-pihat(2,1) << posgps(3,1) - pihat(3,1) << velgps(1,1) - vihat(1,1) << velgps(2,1)-vihat(2,1) << velgps(3,1) - vihat(3,1);
+ errState = K * z;
+ //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);
+}
+
Matrix solaESKF::computeAngles()
{
@@ -157,7 +278,7 @@
//angle error
Matrix qerr(4,1);
- qerr << 1.0f << 0.5f*errState(1,7) << 0.5f*errState(8,1) << 0.5f*errState(9,1);
+ qerr << 1.0f << 0.5f*errState(7,1) << 0.5f*errState(8,1) << 0.5f*errState(9,1);
qhat = quatmultiply(qhat, qerr);
float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
qhat *= (1.0f/ qnorm);