HAPSRG
Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: solaESKF.cpp
- Revision:
- 74:f5fe7fecbd3c
- Parent:
- 73:5770a0d470c0
- Child:
- 75:e2c825cdc511
--- a/solaESKF.cpp Sun Nov 21 08:24:40 2021 +0000
+++ b/solaESKF.cpp Mon Nov 22 09:51: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),magBias(3,1),errState(16,1),Phat(16,16),Q(16,16)
+ :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 << 0.0f << 0.0f << 0.0f;
vihat << 0.0f << 0.0f << 0.0f;
@@ -11,8 +11,6 @@
accBias << 0.0f << 0.0f << 0.0f;
gyroBias << 0.0f << 0.0f << 0.0f;
gravity << 0.0f << 0.0f << 9.8f;
- magBias << 0.0f << 0.0f << 0.0f;
- magRadius = 0.0f;
nState = errState.getRows();
@@ -29,7 +27,7 @@
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,16);//gravity
+ setBlockDiag(Phat,0.00000001f,16,18);//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
@@ -77,10 +75,10 @@
for (int j = 1; j < 4; j++){
A(i+3,j+6) = a2v(i,j);
A(i+3,j+9) = -dcm(i,j);
-
+ A(i+3,j+15) = 1.0f;
}
}
- A(6,16) = 1.0f;
+
//angulat error
A(7,8) = gyrom(3,1);
@@ -100,8 +98,6 @@
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;
}
@@ -123,8 +119,8 @@
for (int i = 1; i < 4; i++){
for (int j = 1; j < 4; j++){
H(i,j+6) = rotgrav(i,j);
+ H(i,j+15) = tdcm(i,j);
}
- H(i,16) = tdcm(i,3);
}
H(1,10) = -1.0f;
@@ -140,79 +136,6 @@
fuseErr2Nominal();
}
-void solaESKF::updateMag(Matrix mag,Matrix R)
-{
- Matrix magm = mag-magBias;
- Matrix dcm(3,3);
- computeDcm(dcm, qhat);
- Matrix tdcm = MatrixMath::Transpose(dcm);
- Matrix rotmag = -dcm*MatrixMath::Matrixcross(magm(1,1),magm(2,1),magm(3,1));
-
- Matrix H(2,nState);
- int nH = H.getRows();
-
- Matrix magned = dcm*magm;
- float hx = sqrt(magned(1,1)*magned(1,1)+magned(2,1)*magned(2,1));
- float hz = sqrt(magned(3,1)*magned(3,1));
- float a = sqrt(magm(1,1)*magm(1,1)+magm(2,1)*magm(2,1)+magm(3,1)*magm(3,1));
-
- H(1,3+6) = rotmag(1,3)-(rotmag(1,3)+rotmag(2,3))/hx;
- H(2,3+6) = rotmag(2,3);
- for(int j = 1; j < 4; j++){
- //H(3,j+6) = rotmag(3,j)-(rotmag(3,j))/hz;
- H(1,j+16) = -dcm(1,j)+(dcm(1,j)+dcm(2,j))/hx;
- H(2,j+16) = -dcm(2,j);
- //H(3,j+16) = -dcm(3,j)+(dcm(3,j))/hz;
- }
- //H(3,20) = 1.0f;
- //H(3,17) = magm(1,1)/a;
- //H(3,18) = magm(2,1)/a;
- //H(3,19) = magm(3,1)/a;
-
- Matrix z(nH,1);
- z << -(magned(1,1) - hx) << -magned(2,1);
- //twelite.printf("%f %f %f %f\r\n",-(magned(1,1) - hx),-magned(2,1),-(magned(3,1) - hz),-(magRadius-a));
- Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
-
- /*
- Matrix r3(3,1);
- r3 << tdcm(1,3)<< tdcm(2,3) << tdcm(3,3);
- Matrix kpart = r3*MatrixMath::Transpose(r3);
- Matrix Pm(nState,nState);
- for(int i = 7; i<10; i++){
- for(int j = 7;j<10;j++){
- Pm(i,j) = Phat(i,j);
- }
- }
- for(int i = 17; i<nState+1; i++){
- for(int j = 17;j<nState+1;j++){
- Pm(i,j) = Phat(i,j);
- }
- }
-
- Matrix K = (Pm*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Pm*MatrixMath::Transpose(H)+R);
- Matrix Kmod(3,nH);
- for(int i = 1; i<4; i++){
- for(int j = 1;j<nH+1;j++){
- Kmod(i,j) = K(i+6,j);
- }
- }
- Kmod = kpart*Kmod;
- for(int i = 1; i<nState+1; i++){
- for(int j = 1;j<nH;j++){
- if(i>6 && i<10){
- K(i,j) = Kmod(i-6,j);
- }else if(i<17){
- K(i,j) = 0.0f;
- }
- }
- }
- */
- errState = K * z;
- Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
-
- fuseErr2Nominal();
-}
void solaESKF::updateHeading(float a,Matrix R)
{
@@ -285,115 +208,46 @@
H(1,j+6) = Hpart(1,j);
}
- float psi = atan2f(qhat(2,1)*qhat(3,1) + qhat(1,1)*qhat(4,1), 0.5f - qhat(3,1)*qhat(3,1) - qhat(4,1)*qhat(4,1));
+ const float psi = atan2f(qhat(2,1)*qhat(3,1) + qhat(1,1)*qhat(4,1), 0.5f - qhat(3,1)*qhat(3,1) - qhat(4,1)*qhat(4,1));
Matrix z(1,1);
- z << atan2(sin(a-psi),cos(a-psi));
+ z << atan2f(sin(a-psi),cos(a-psi));
Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
errState = K * z;
Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
fuseErr2Nominal();
}
-void solaESKF::updateImuConstraints(Matrix acc,Matrix mag,Matrix R)
+void solaESKF::updateGPSPosition(Matrix posgps,float palt,Matrix R)
{
- Matrix accm = acc - accBias;
- Matrix dcm(3,3);
- computeDcm(dcm, qhat);
- Matrix tdcm = MatrixMath::Transpose(dcm);
- Matrix tdcm_g = tdcm*gravity;
- Matrix rotgrav = MatrixMath::Matrixcross(tdcm_g(1,1),tdcm_g(2,1),tdcm_g(3,1));
- Matrix rotmag = -dcm*MatrixMath::Matrixcross(mag(1,1),mag(2,1),mag(3,1));
-
- Matrix H(6,nState);
- for (int i = 1; i < 3; i++){
- for (int j = 1; j < 4; j++){
- H(i,j+6) = rotgrav(i,j);
- }
- H(i,16) = tdcm(i,3);
- }
-
- H(1,10) = -1.0f;
- H(2,11) = -1.0f;
- H(3,12) = -1.0f;
-
- Matrix magned = dcm*mag;
- float hx = sqrt(magned(1,1)*magned(1,1)+magned(2,1)*magned(2,1));
- float hz = sqrt(magned(3,1)*magned(3,1));
+ Matrix H(3,nState);
+ H(1,1) = 1.0f;
+ H(2,2) = 1.0f;
+ H(3,3) = 1.0f;
- for(int j = 3; j < 4; j++){
- H(4,j+6) = rotmag(1,j)-(rotmag(1,j)+rotmag(2,j))/hx;
- //H(4,j+16) = -dcm(1,j)+(dcm(1,j)+dcm(2,j))/hx;
- H(5,j+6) = rotmag(2,j);
- //H(5,j+16) = -dcm(2,j);
- H(6,j+6) = rotmag(3,j)-(rotmag(3,j))/hz;
- }
+ //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 r3(3,1);
- r3 << tdcm(1,3)<< tdcm(2,3) << tdcm(3,3);
- Matrix kpart = r3*MatrixMath::Transpose(r3);
- Matrix Kmod(3,3);
- for(int i = 1; i<4; i++){
- for(int j = 1;j<4;j++){
- Kmod(i,j) = K(i+6,j+3);
- }
- }
- Kmod = kpart*Kmod;
- */
- for(int i = 1; i<nState+1; i++){
- for(int j = 4;j<7;j++){
- if(i>6 && i<10){
- //K(i,j) = Kmod(i-6,j-3);
- }else{
- K(i,j) = 0.0f;
- }
- }
- }
-
- Matrix zacc = -accm-tdcm*gravity;
- Matrix z(6,1);
- z << zacc(1,1) << zacc(2,1) << zacc(3,1) << -(magned(1,1) - hx) << -magned(2,1) << -(magned(3,1) - hz);
+ 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();
}
-
-void solaESKF::updateOtherConstraints(Matrix mag,float palt,Matrix R)
-{
- Matrix magm = mag - magBias;
- float a = sqrt(magm(1,1)*magm(1,1)+magm(2,1)*magm(2,1)+magm(3,1)*magm(3,1));
- Matrix H(2,nState);
- H(1,20) = -1.0f;
- H(1,17) = -magm(1,1)/a;
- H(1,18) = -magm(2,1)/a;
- H(1,19) = -magm(3,1)/a;
- H(2,3) = 1.0f;
-
- Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
-
- Matrix z(2,1);
- z << -(a-magRadius) << palt-pihat(3,1);
- //twelite.printf("%f %f\r\n",hx,(zmag(1,1) - hx));
- errState = K * z;
- Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
-
- fuseErr2Nominal();
-}
-
-
void solaESKF::updateGPS(Matrix posgps,float palt,Matrix velgps,Matrix R)
{
- Matrix H(5,nState);
+ 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 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)<< velgps(3,1)-vihat(3,1);
errState = K * z;
Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
fuseErr2Nominal();
@@ -441,9 +295,9 @@
gyroBias(3,1) += errState(15,1);
//gravity bias
- gravity(1,1) = 0.0f;
- gravity(2,1) = 0.0f;
- gravity(3,1) += errState(16,1);
+ gravity(1,1) += errState(16,1);
+ gravity(2,1) += errState(17,1);
+ gravity(3,1) += errState(18,1);
@@ -531,14 +385,7 @@
{
return gravity;
}
-Matrix solaESKF::getMagBias()
-{
- return magBias;
-}
-float solaESKF::getMagRadius()
-{
- return magRadius;
-}
+
Matrix solaESKF::getErrState()
{
return errState;
@@ -566,15 +413,7 @@
}
void solaESKF::setPhatGravity(float val)
{
- setBlockDiag(Phat,val,16,16);
-}
-void solaESKF::setPhatMagBias(float val)
-{
- setBlockDiag(Phat,val,17,19);
-}
-void solaESKF::setPhatMagRadius(float val)
-{
- setBlockDiag(Phat,val,20,20);
+ setBlockDiag(Phat,val,16,18);
}
@@ -594,14 +433,6 @@
{
setBlockDiag(Q,val,13,15);
}
-void solaESKF::setQMagBias(float val)
-{
- setBlockDiag(Q,val,17,19);
-}
-void solaESKF::setQMagRadius(float val)
-{
- setBlockDiag(Q,val,20,20);
-}
void solaESKF::setDiag(Matrix& mat, float val){
@@ -623,18 +454,6 @@
//pihat(3,1) = pi_z;
}
-float solaESKF::wrap_pi(float x) {
- const float pi = 3.141592f;
- const float range = pi + pi;
- twelite.printf("1 : %f ",x);
-
- if (x < -pi) {
- x += range * ((-pi - x) / range + 1);
- }
-
- twelite.printf("2 : %f\r\n",-pi + fmodf( (x + pi),range));
- return -pi + fmodf((x + pi),range);
-}
/*
void solaESKF::updateAccConstraints(Matrix acc,float palt,Matrix R)
{