HAPSRG
Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: solaESKF.cpp
- Revision:
- 56:c10f1168bd4a
- Parent:
- 55:21611d4cf7e8
- Child:
- 58:93ba28cf5cb3
--- a/solaESKF.cpp Mon Nov 08 09:19:55 2021 +0000
+++ b/solaESKF.cpp Wed Nov 10 05:21:36 2021 +0000
@@ -3,7 +3,7 @@
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)
+ :pihat(3,1),vihat(3,1),qhat(4,1),accBias(3,1),gyroBias(3,1),gravity(3,1),magField(3,1),errState(19,1),Phat(19,19),Q(19,19)
{
pihat << 0.0f << 0.0f << 0.0f;
vihat << 0.0f << 0.0f << 0.0f;
@@ -11,10 +11,11 @@
accBias << 0.0f << 0.0f << 0.0f;
gyroBias << 0.0f << 0.0f << 0.0f;
gravity << 0.0f << 0.0f << 9.8f;
+ magField << -0.2f << -0.0f << 0.0f;
- for (int i = 1; i < 19; i++){
+ for (int i = 1; i < 20; i++){
errState(i,1) = 0.0f;
- for (int j = 1; j < 19; j++){
+ for (int j = 1; j < 20; j++){
Phat(i,j) = 0.0f;
Q(i,j) = 0.0f;
}
@@ -28,11 +29,13 @@
setBlockDiag(Phat,0.1f,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(Phat,0.00000001f,16,18);//gravity
+ setBlockDiag(Phat,0.1f,19,19);//gravity
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 //positionとgravityはQ項なし
+ setBlockDiag(Q,0.001f,13,15);//gyro bias
+ setBlockDiag(Q,0.001f,19,19);//mag field //positionとgravityはQ項なし
}
@@ -103,149 +106,66 @@
Phat = Fx*Phat*MatrixMath::Transpose(Fx)+Qd;
}
-void solaESKF::updateAccConstraints(Matrix acc,float palt,Matrix R)
+void solaESKF::updateImuConstraints(Matrix acc,Matrix mag,Matrix R)
{
Matrix accm = acc - accBias;
- Matrix tdcm(3,3);
- computeDcm(tdcm, qhat);
- tdcm = MatrixMath::Transpose(tdcm);
+ Matrix dcm(3,3);
+ computeDcm(dcm, qhat);
+ Matrix tdcm = MatrixMath::Transpose(dcm);
Matrix tdcm_g = tdcm*gravity;
Matrix a2v = MatrixMath::Matrixcross(tdcm_g(1,1),tdcm_g(2,1),tdcm_g(3,1));
+ Matrix a2v2 = -dcm*MatrixMath::Matrixcross(mag(1,1),mag(2,1),mag(3,1));
- Matrix H(4,nState);
- for (int i = 1; i < 4; i++){
- for (int j = 1; j < 4; j++){
+ Matrix H(5,nState);
+ for (int j = 1; j < 4; j++){
+ for (int i = 1; i < 4; i++){
H(i,j+6) = a2v(i,j);
H(i,j+15) = tdcm(i,j);
}
+ H(4,j+6) = a2v2(1,j);
+ H(5,j+6) = a2v2(2,j);
}
+
H(1,10) = -1.0f;
H(2,11) = -1.0f;
H(3,12) = -1.0f;
- H(4,3) = 1.0f;
+ H(4,19) = 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 zacc = -accm-tdcm*gravity;
- Matrix z(4,1);
- z << zacc(1,1) << zacc(2,1) << zacc(3,1) << palt - pihat(3,1);
+ Matrix zmag = -dcm*mag-magField;
+ Matrix z(5,1);
+ z << zacc(1,1) << zacc(2,1) << zacc(3,1) << zmag(1,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::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 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);
-}
-void solaESKF::updateMag(Matrix mag,Matrix R)
-{
- float magnorm = sqrt(mag(1,1)*mag(1,1)+mag(2,1)*mag(2,1)+mag(3,1)*mag(3,1));
- Matrix magm = mag;
- Matrix dcm(3,3);
- computeDcm(dcm, qhat);
- Matrix a2v = -dcm*MatrixMath::Matrixcross(magm(1,1),magm(2,1),magm(3,1));
-
- Matrix H(2,nState);
- for (int j = 1; j < 4; j++){
- H(1,j+6) = a2v(1,j);
- H(2,j+6) = a2v(2,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 K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
-
- Matrix zmag = dcm*magm;
- Matrix z(2,1);
- z << 0.5f-zmag(1,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);
-}
-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);
-}
-
-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);
-}
-
-void solaESKF::updateGPS(Matrix posgps,Matrix velgps,Matrix R)
+void solaESKF::updateGPS(Matrix posgps,float palt,Matrix velgps,Matrix R)
{
Matrix H(5,nState);
H(1,1) = 1.0f;
H(2,2) = 1.0f;
- H(3,4) = 1.0f;
- H(4,5) = 1.0f;
- H(5,6) = 1.0f;
+ H(3,3) = 1.0f;
+ H(4,4) = 1.0f;
+ H(5,5) = 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(5,1);
- z << posgps(1,1) - pihat(1,1) << posgps(2,1)-pihat(2,1) << velgps(1,1) - vihat(1,1) << velgps(2,1)-vihat(2,1) << velgps(3,1) - vihat(3,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);
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();
}
Matrix solaESKF::computeAngles()
@@ -293,7 +213,12 @@
gravity(2,1) += errState(17,1);
gravity(3,1) += errState(18,1);
- for (int i = 1; i < 19; i++){
+ //gravity bias
+ magField(1,1) += errState(19,1);
+ magField(2,1) = 0.0f;
+ magField(3,1) = 0.0f;
+
+ for (int i = 1; i < 20; i++){
errState(i,1) = 0.0f;
}
@@ -376,6 +301,10 @@
{
return gravity;
}
+Matrix solaESKF::getMagField()
+{
+ return magField;
+}
Matrix solaESKF::getErrState()
{
return errState;
@@ -405,6 +334,10 @@
{
setBlockDiag(Phat,val,16,18);
}
+void solaESKF::setPhatMagField(float val)
+{
+ setBlockDiag(Phat,val,19,19);
+}
void solaESKF::setQVelocity(float val)
@@ -423,6 +356,10 @@
{
setBlockDiag(Q,val,13,15);
}
+void solaESKF::setQMagField(float val)
+{
+ setBlockDiag(Q,val,19,19);
+}
void solaESKF::setDiag(Matrix& mat, float val){
@@ -439,5 +376,143 @@
void solaESKF::setPihat(float pi_x, float pi_y, float pi_z)
{
- pihat << pi_x << pi_y << pi_z;
-}
\ No newline at end of file
+ pihat(1,1) = pi_x;
+ pihat(2,1) = pi_y;
+ pihat(3,1) = pi_z;
+}
+void solaESKF::setMagField(float hx, float hy, float hz)
+{
+ magField(1,1) = -sqrt(hx*hx+hy*hy);
+ magField(2,1) = 0.0f;
+ magField(3,1) = 0.0f;
+}
+
+/*
+void solaESKF::updateAccConstraints(Matrix acc,float palt,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(4,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;
+ H(4,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 zacc = -accm-tdcm*gravity;
+ Matrix z(4,1);
+ z << zacc(1,1) << zacc(2,1) << zacc(3,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::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 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);
+ fuseErr2Nominal();
+}
+void solaESKF::updateMag(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(2,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,20) = 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(2,1);
+ z << zmag(1,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 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