HAPSRG
Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: solaESKF.cpp
- Revision:
- 59:03fe5e16a33c
- Parent:
- 58:93ba28cf5cb3
- Child:
- 60:f4b4231a8d3f
--- a/solaESKF.cpp Fri Nov 12 09:03:10 2021 +0000
+++ b/solaESKF.cpp Mon Nov 15 13:41:40 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),magField(3,1),errState(19,1),Phat(19,19),Q(19,19)
+ :pihat(3,1),vihat(3,1),qhat(4,1),accBias(3,1),gyroBias(3,1),gravity(3,1),magBias(3,1),errState(20,1),Phat(20,20),Q(20,20)
{
pihat << 0.0f << 0.0f << 0.0f;
vihat << 0.0f << 0.0f << 0.0f;
@@ -11,7 +11,8 @@
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;
+ magBias << 0.0f << 0.0f << 0.0f;
+ magRadius = -0.5f;
nState = errState.getRows();
@@ -23,21 +24,20 @@
}
}
-
-
-
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.1f,10,12);//acc bias
setBlockDiag(Phat,0.1f,13,15);//gyro bias
- setBlockDiag(Phat,0.00000001f,16,18);//gravity
- setBlockDiag(Phat,0.001f,19,19);//H0
+ setBlockDiag(Phat,0.00000001f,16,16);//gravity
+ setBlockDiag(Phat,0.001f,17,19);//magBias
+ setBlockDiag(Phat,0.001f,20,20);//magRadius
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.0001f,19,19);//mag field //positionとgravityはQ項なし
+ setBlockDiag(Q,0.001f,17,19);//magRadius
+ setBlockDiag(Q,0.0001f,20,20);//magRadius //positionとgravityはQ項なし
}
@@ -85,9 +85,7 @@
}
}
- A(4,16) = 1.0f;
- A(5,17) = 1.0f;
- A(6,18) = 1.0f;
+ A(6,16) = 1.0f;
//angulat error
A(7,8) = gyrom(3,1);
@@ -102,161 +100,82 @@
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;
-
+ for (int i = 1; i < nState+1; i++){
+ if(i>3 && i<10){
+ Q(i,i) *=att_dt;
+ }else if(i>9 && i<16){
+ Q(i,i) *= att_dt*att_dt;
+ }else if(i>16 && i<nState+1){
+ Q(i,i) *=att_dt*att_dt;
+ }else{
+ Q(i,i) = 0.0f;
+ }
+ }
errState = Fx * errState;
- Phat = Fx*Phat*MatrixMath::Transpose(Fx)+Qd;
+ Phat = Fx*Phat*MatrixMath::Transpose(Fx)+Q;
}
void solaESKF::updateImuConstraints(Matrix acc,Matrix mag,Matrix R)
{
Matrix accm = acc - accBias;
+ Matrix magm = mag - magBias;
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 rotgrav = MatrixMath::Matrixcross(tdcm_g(1,1),tdcm_g(2,1),tdcm_g(3,1));
+ Matrix rotmag = -dcm*MatrixMath::Matrixcross(magm(1,1),magm(2,1),magm(3,1));
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);
+ for (int i = 1; i < 4; i++){
+ for (int j = 1; j < 4; j++){
+ H(i,j+6) = rotgrav(i,j);
}
- H(4,j+6) = a2v2(1,j);
- H(5,j+6) = a2v2(2,j);
- }
- for (int i = 1; i < 4; i++){
-
+ H(i,16) = tdcm(i,3);
}
H(1,10) = -1.0f;
H(2,11) = -1.0f;
H(3,12) = -1.0f;
- H(4,19) = 1.0f;
+
+ Matrix magned = dcm*magm;
+ float hx = sqrt(magned(1,1)*magned(1,1)+magned(2,1)*magned(2,1));
- //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));
+ for(int j = 1; j < 4; j++){
+ H(4,j+6) = rotmag(1,j)+2.0f*(rotmag(1,j)+rotmag(2,j))/hx;
+ H(4,j+16) = -dcm(1,j)+2.0f*dcm(1,j)/hx;
+ H(5,j+6) = rotmag(2,j);
+ H(5,j+16) = -dcm(2,j)+2.0f*dcm(2,j)/hx;
+ }
+
Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
Matrix zacc = -accm-tdcm*gravity;
- Matrix zmag = -dcm*mag-magField;
+ Matrix zmag = dcm*magm;
Matrix z(5,1);
- z << zacc(1,1) << zacc(2,1) << zacc(3,1) << zmag(1,1) << zmag(2,1);
+ z << zacc(1,1) << zacc(2,1) << zacc(3,1) << -(zmag(1,1) - hx) << 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::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(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)
+void solaESKF::updateGPS(Matrix posgps,float palt,Matrix velgps,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);
+ Matrix H(5,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(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);
+ 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);
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);
- //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
- fuseErr2Nominal();
-}
Matrix solaESKF::computeAngles()
{
@@ -299,14 +218,15 @@
gyroBias(3,1) += errState(15,1);
//gravity bias
- gravity(1,1) += errState(16,1);
- gravity(2,1) += errState(17,1);
- gravity(3,1) += errState(18,1);
+ gravity(1,1) = 0.0f;
+ gravity(2,1) = 0.0f;
+ gravity(3,1) += errState(16,1);
- //Magnetic Field
- magField(1,1) += errState(19,1);
- magField(2,1) = 0.0f;
- magField(3,1) = 0.0f;
+ //Magnetic bias
+ magBias(1,1) += errState(17,1);
+ magBias(2,1) += errState(18,1);
+ magBias(3,1) += errState(19,1);
+ magRadius += errState(20,1);
for (int i = 1; i < nState+1; i++){
errState(i,1) = 0.0f;
@@ -391,9 +311,13 @@
{
return gravity;
}
-Matrix solaESKF::getMagField()
+Matrix solaESKF::getMagBias()
{
- return magField;
+ return magBias;
+}
+float solaESKF::getMagRadius()
+{
+ return magRadius;
}
Matrix solaESKF::getErrState()
{
@@ -422,11 +346,15 @@
}
void solaESKF::setPhatGravity(float val)
{
- setBlockDiag(Phat,val,16,18);
+ setBlockDiag(Phat,val,16,16);
}
-void solaESKF::setPhatMagField(float val)
+void solaESKF::setPhatMagBias(float val)
{
- setBlockDiag(Phat,val,19,19);
+ setBlockDiag(Phat,val,17,19);
+}
+void solaESKF::setPhatMagRadius(float val)
+{
+ setBlockDiag(Phat,val,20,20);
}
@@ -446,9 +374,13 @@
{
setBlockDiag(Q,val,13,15);
}
-void solaESKF::setQMagField(float val)
+void solaESKF::setQMagBias(float val)
{
- setBlockDiag(Q,val,19,19);
+ setBlockDiag(Q,val,17,19);
+}
+void solaESKF::setQMagRadius(float val)
+{
+ setBlockDiag(Q,val,20,20);
}
@@ -470,13 +402,6 @@
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)
{
@@ -566,6 +491,80 @@
//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,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(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::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);
+ //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