HAPSRG
Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: solaESKF.cpp
- Revision:
- 58:93ba28cf5cb3
- Parent:
- 56:c10f1168bd4a
- Child:
- 59:03fe5e16a33c
--- a/solaESKF.cpp Wed Nov 10 06:33:45 2021 +0000
+++ b/solaESKF.cpp Fri Nov 12 09:03:10 2021 +0000
@@ -12,17 +12,19 @@
gyroBias << 0.0f << 0.0f << 0.0f;
gravity << 0.0f << 0.0f << 9.8f;
magField << -0.2f << -0.0f << 0.0f;
+
+ nState = errState.getRows();
- for (int i = 1; i < 20; i++){
+ for (int i = 1; i < nState+1; i++){
errState(i,1) = 0.0f;
- for (int j = 1; j < 20; j++){
+ for (int j = 1; j < nState+1; j++){
Phat(i,j) = 0.0f;
Q(i,j) = 0.0f;
}
}
- nState = errState.getRows();
+
setBlockDiag(Phat,0.1f,1,3);//position
setBlockDiag(Phat,0.1f,4,6);//velocity
@@ -30,12 +32,12 @@
setBlockDiag(Phat,0.1f,10,12);//acc bias
setBlockDiag(Phat,0.1f,13,15);//gyro bias
setBlockDiag(Phat,0.00000001f,16,18);//gravity
- setBlockDiag(Phat,0.1f,19,19);//gravity
+ setBlockDiag(Phat,0.001f,19,19);//H0
setBlockDiag(Q,0.00025f,4,6);//velocity
setBlockDiag(Q,0.005f/57.0f,7,9);//angle error
setBlockDiag(Q,0.001f,10,12);//acc bias
setBlockDiag(Q,0.001f,13,15);//gyro bias
- setBlockDiag(Q,0.001f,19,19);//mag field //positionとgravityはQ項なし
+ setBlockDiag(Q,0.0001f,19,19);//mag field //positionとgravityはQ項なし
}
@@ -125,6 +127,9 @@
H(4,j+6) = a2v2(1,j);
H(5,j+6) = a2v2(2,j);
}
+ for (int i = 1; i < 4; i++){
+
+ }
H(1,10) = -1.0f;
H(2,11) = -1.0f;
@@ -145,22 +150,107 @@
fuseErr2Nominal();
}
-
-void solaESKF::updateGPS(Matrix posgps,float palt,Matrix velgps,Matrix R)
+void solaESKF::updateMag(Matrix mag,float palt, Matrix R)
{
+ Matrix dcm(3,3);
+ computeDcm(dcm, qhat);
+ Matrix a2v = -dcm*MatrixMath::Matrixcross(mag(1,1),mag(2,1),mag(3,1));
- Matrix H(5,nState);
+ Matrix H(3,nState);
+ for (int i = 1; i < 3; i++){
+ for (int j = 1; j < 4; j++){
+ H(i,j+6) = a2v(i,j);
+ }
+ }
+ H(1,19) = 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+10.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A));
+ Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
+ Matrix zmag = -dcm*mag-magField;
+ Matrix z(3,1);
+ z << zmag(1,1) << zmag(2,1) << palt - 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);
+
+ fuseErr2Nominal();
+}
+
+
+void solaESKF::updateGPS(Matrix posgps,float palt,Matrix velgps,Matrix mag,Matrix R)
+{
+ Matrix dcm(3,3);
+ computeDcm(dcm, qhat);
+ Matrix a2v = -dcm*MatrixMath::Matrixcross(mag(1,1),mag(2,1),mag(3,1));
+
+ 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;
+ for (int j = 1; j < 4; j++){
+ H(6,j+6) = a2v(2,j);
+ }
+
+ Matrix zmag = -dcm*mag-magField;
//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(5,1);
- z << posgps(1,1) - pihat(1,1) << posgps(2,1)-pihat(2,1) << palt - pihat(3,1) << velgps(1,1) - vihat(1,1) << velgps(2,1)-vihat(2,1);
+ Matrix z(6,1);
+ z << posgps(1,1) - pihat(1,1) << posgps(2,1)-pihat(2,1) << palt - pihat(3,1) << velgps(1,1) - vihat(1,1) << velgps(2,1)-vihat(2,1)<< zmag(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);
+ fuseErr2Nominal();
+}
+void solaESKF::updateGPSVelocity(Matrix velgps,Matrix mag,Matrix R)
+{
+
+ Matrix dcm(3,3);
+ computeDcm(dcm, qhat);
+ Matrix a2v = -dcm*MatrixMath::Matrixcross(mag(1,1),mag(2,1),mag(3,1));
+
+
+ Matrix H(3,nState);
+ H(1,4) = 1.0f;
+ H(2,5) = 1.0f;
+
+ for (int j = 1; j < 4; j++){
+ H(3,j+6) = a2v(2,j);
+ }
+ //H(3,19) = 1.0f;
+
+ Matrix zmag = -dcm*mag-magField;
+
+ //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) << zmag(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);
+ fuseErr2Nominal();
+}
+
+void solaESKF::updateGPSPosition(Matrix posgps,float palt,Matrix R)
+{
+ Matrix H(3,nState);
+ H(1,1) = 1.0f;
+ 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 z(3,1);
+ z << posgps(1,1) - pihat(1,1) << posgps(2,1)-pihat(2,1) << palt - 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);
@@ -213,12 +303,12 @@
gravity(2,1) += errState(17,1);
gravity(3,1) += errState(18,1);
- //gravity bias
- magField(1,1) += errState(19,1);
+ //Magnetic Field
+ magField(1,1) += errState(19,1);
magField(2,1) = 0.0f;
magField(3,1) = 0.0f;
- for (int i = 1; i < 20; i++){
+ for (int i = 1; i < nState+1; i++){
errState(i,1) = 0.0f;
}
@@ -374,11 +464,11 @@
}
}
-void solaESKF::setPihat(float pi_x, float pi_y, float pi_z)
+void solaESKF::setPihat(float pi_x, float pi_y)
{
pihat(1,1) = pi_x;
pihat(2,1) = pi_y;
- pihat(3,1) = pi_z;
+ //pihat(3,1) = pi_z;
}
void solaESKF::setMagField(float hx, float hy, float hz)
{
@@ -477,42 +567,5 @@
fuseErr2Nominal();
}
-void solaESKF::updateGPSVelocity(Matrix velgps,Matrix R)
-{
-
- Matrix H(3,nState);
- 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 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);
- fuseErr2Nominal();
-}
-void solaESKF::updateGPSPosition(Matrix posgps,Matrix R)
-{
- Matrix H(2,nState);
- H(1,1) = 1.0f;
- 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 z(2,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);
- fuseErr2Nominal();
-}
*/
\ No newline at end of file