HAPSRG
Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: ScErrStateEKF.cpp
- Revision:
- 26:73c3f58b9d70
- Parent:
- 25:07ac5c6cd61c
- Child:
- 27:a74f101ba40a
--- a/ScErrStateEKF.cpp Wed Aug 25 07:01:05 2021 +0000
+++ b/ScErrStateEKF.cpp Tue Sep 07 06:19:29 2021 +0000
@@ -8,7 +8,7 @@
using namespace std;
ScErrStateEKF::ScErrStateEKF()
- :qhat(4,1),vihat(3,1), errState(12,1), Phat(12,12), Q(12,12), Ra(3,3), Rm(3,3), Qab(3,3),Rgsc(2,2),Rvsc(2,2),accBias(0.0f,0.0f,0.0f),gyroBias(0.0f,0.0f,0.0f)
+ :qhat(4,1),vihat(3,1), errState(12,1), Phat(12,12), Q(12,12), Ra(3,3), Rgps(2,2), Rm(3,3), Qab(3,3),Rgsc(2,2),Rvsc(2,2),accBias(0.0f,0.0f,0.0f),gyroBias(0.0f,0.0f,0.0f)
{
nState = errState.getRows();
qhat << 1.0f << 0.0f << 0.0f << 0.0f;
@@ -31,6 +31,8 @@
setDiag(Rm,5.0f);
+ setDiag(Rgps,0.5f);
+
for(int i = 0; i<10;i++){
histffunc[i] = 0.0f;
}
@@ -226,32 +228,6 @@
Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Rgsc)*MatrixMath::Transpose(K);
}
-void ScErrStateEKF::updateVelocityConstraints()
-{
-
- Matrix dcm(3,3);
- computeDcm(dcm, qhat);
- Matrix H(2,nState);
-
- Matrix nomVb = dcm*vihat;
- Matrix qeterm = -2.0f*MatrixMath::Matrixcross(nomVb(1,1),nomVb(2,1),nomVb(3,1));
- for(int j = 1;j<4;j++){
- H(1,j) = -qeterm(1,j);
- H(2,j) = -qeterm(3,j);
- //H(3,j) = -qeterm(3,j);
- H(1,9+j) = -dcm(1,j);
- H(2,9+j) = -dcm(3,j);
- //H(3,9+j) = -dcm(3,j);
- }
-
- Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Rvsc);
- Matrix z(2,1);
- z << nomVb(1,1) << nomVb(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*(Rvsc)*MatrixMath::Transpose(K);
-
-}
void ScErrStateEKF::updateMeasures(Vector3 gyro, Vector3 acc, Vector3 accref)
{
@@ -372,53 +348,6 @@
Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(R)*MatrixMath::Transpose(K);
}
-void ScErrStateEKF::updateConstraints(Vector3 gyro)
-{
-
- gyro -=gyroBias;
-
- Matrix dcm(3,3);
- computeDcm(dcm, qhat);
-
- Matrix qeterm = -2.0f*MatrixMath::Transpose(dcm)*MatrixMath::Matrixcross(gyro.x,gyro.y,gyro.z);
- Matrix H(4,nState);
- for(int i = 1; i<3; i++){
- for(int j = 1;j<4;j++){
- H(i,j) = qeterm(i,j);
- }
- }
- H(1,4) = 1.0f*(dcm(1,1));
- H(1,5) = 1.0f*(dcm(2,1));
- H(1,6) = 1.0f*(dcm(3,1));
- H(2,4) = 1.0f*(dcm(1,2));
- H(2,5) = 1.0f*(dcm(2,2));
- H(2,6) = 1.0f*(dcm(3,2));
-
- Matrix nomVb = dcm*vihat;
- qeterm = -2.0f*MatrixMath::Matrixcross(nomVb(1,1),nomVb(2,1),nomVb(3,1));
- for(int j = 1;j<4;j++){
- H(3,j) = -qeterm(1,j);
- H(4,j) = -qeterm(3,j);
- //H(3,j) = -qeterm(3,j);
- H(3,9+j) = -dcm(1,j);
- H(4,9+j) = -dcm(3,j);
- //H(3,9+j) = -dcm(3,j);
- }
-
-
- Matrix R(4,4);
- R(1,1) = Rgsc(1,1);
- R(2,2) = Rgsc(2,1);
- R(3,3) = Rvsc(1,1);
- R(4,4) = Rvsc(2,1);
- //R(5,5) = Rvsc(3,1);
- Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
- Matrix z(4,1);
- z <<dcm(1,1)*gyro.x+dcm(2,1)*gyro.y+dcm(3,1)*gyro.z<< dcm(1,2)*gyro.x+dcm(2,2)*gyro.y+dcm(3,2)*gyro.z << nomVb(1,1) << nomVb(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);
-}
@@ -444,51 +373,17 @@
Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Ra*100.0f)*MatrixMath::Transpose(K);
}
-void ScErrStateEKF::updateMagMeasures(Vector3 mag)
+void ScErrStateEKF::updateGPSVelocity(float speed, float direction)
{
- //mag = mag/mag.Norm();
- Matrix dcm(3,3);
- computeDcm(dcm, qhat);
-
- Matrix magvec(3,1);
- magvec(1,1) = mag.x;
- magvec(2,1) = mag.y;
- magvec(3,1) = mag.z;
-
- Matrix magnedvec = MatrixMath::Transpose(dcm)*magvec;
- Matrix magrefmod(3,1);
- magrefmod(1,1) = sqrt(magnedvec(1,1)*magnedvec(1,1)+magnedvec(2,1)*magnedvec(2,1));
- magrefmod(2,1) = 0.0f;
- magrefmod(3,1) = magnedvec(3,1);
-
- Matrix mvec = dcm*magrefmod ;
Matrix H(3,nState);
- H(1,2) = -2.0f*magvec(3,1);
- H(1,3) = 2.0f*magvec(2,1);
- H(2,1) = 2.0f*magvec(3,1);
- H(2,3) = -2.0f*magvec(1,1);
- H(3,1) = -2.0f*magvec(2,1);
- H(3,2) = 2.0f*magvec(1,1);
- Matrix Pm(nState,nState);
- for(int i = 1; i<4; i++){
- for(int j = 1;j<4;j++){
- Pm(i,j) = Phat(i,j);
- }
- }
- Matrix r3(3,1);
- r3 << dcm(1,3)<< dcm(2,3) << dcm(3,3);
- Matrix kpart = r3*MatrixMath::Transpose(r3);
- Matrix Kmod(nState,nState);
- for(int i = 1; i<4; i++){
- for(int j = 1;j<4;j++){
- Kmod(i,j) = kpart(i,j);
- }
- }
-
- Matrix K = Kmod*(Pm*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Pm*MatrixMath::Transpose(H)+Rm);
- Matrix z = magvec-mvec;
- errState = errState + K * (z-H*errState);
- Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Rm)*MatrixMath::Transpose(K);
+ H(1,9) = 1.0f;
+ H(2,10) = 1.0f;
+ Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Rgps);
+ Matrix z(2,1);
+ z << speed * cos(direction / 180.0f * M_PI) - vihat(1,1) << speed * sin(direction / 180.0f * M_PI)-vihat(2,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*(Rgps)*MatrixMath::Transpose(K);
}
void ScErrStateEKF::updateCenteredGyroBiasCorrection(float (&cBg)[3])
@@ -843,6 +738,80 @@
vb.z = vbmat(3,1);
}
+
+void ScErrStateEKF::updateMagMeasures(Vector3 mag)
+{
+ //mag = mag/mag.Norm();
+ Matrix dcm(3,3);
+ computeDcm(dcm, qhat);
+
+ Matrix magvec(3,1);
+ magvec(1,1) = mag.x;
+ magvec(2,1) = mag.y;
+ magvec(3,1) = mag.z;
+
+ Matrix magnedvec = MatrixMath::Transpose(dcm)*magvec;
+ Matrix magrefmod(3,1);
+ magrefmod(1,1) = sqrt(magnedvec(1,1)*magnedvec(1,1)+magnedvec(2,1)*magnedvec(2,1));
+ magrefmod(2,1) = 0.0f;
+ magrefmod(3,1) = magnedvec(3,1);
+
+ Matrix mvec = dcm*magrefmod ;
+ Matrix H(3,nState);
+ H(1,2) = -2.0f*magvec(3,1);
+ H(1,3) = 2.0f*magvec(2,1);
+ H(2,1) = 2.0f*magvec(3,1);
+ H(2,3) = -2.0f*magvec(1,1);
+ H(3,1) = -2.0f*magvec(2,1);
+ H(3,2) = 2.0f*magvec(1,1);
+ Matrix Pm(nState,nState);
+ for(int i = 1; i<4; i++){
+ for(int j = 1;j<4;j++){
+ Pm(i,j) = Phat(i,j);
+ }
+ }
+ Matrix r3(3,1);
+ r3 << dcm(1,3)<< dcm(2,3) << dcm(3,3);
+ Matrix kpart = r3*MatrixMath::Transpose(r3);
+ Matrix Kmod(nState,nState);
+ for(int i = 1; i<4; i++){
+ for(int j = 1;j<4;j++){
+ Kmod(i,j) = kpart(i,j);
+ }
+ }
+
+ Matrix K = Kmod*(Pm*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Pm*MatrixMath::Transpose(H)+Rm);
+ Matrix z = magvec-mvec;
+ errState = errState + K * (z-H*errState);
+ Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Rm)*MatrixMath::Transpose(K);
+}
+
+void ScErrStateEKF::updateVelocityConstraints()
+{
+
+ Matrix dcm(3,3);
+ computeDcm(dcm, qhat);
+ Matrix H(2,nState);
+
+ Matrix nomVb = dcm*vihat;
+ Matrix qeterm = -2.0f*MatrixMath::Matrixcross(nomVb(1,1),nomVb(2,1),nomVb(3,1));
+ for(int j = 1;j<4;j++){
+ H(1,j) = -qeterm(1,j);
+ H(2,j) = -qeterm(3,j);
+ //H(3,j) = -qeterm(3,j);
+ H(1,9+j) = -dcm(1,j);
+ H(2,9+j) = -dcm(3,j);
+ //H(3,9+j) = -dcm(3,j);
+ }
+
+ Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Rvsc);
+ Matrix z(2,1);
+ z << nomVb(1,1) << nomVb(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*(Rvsc)*MatrixMath::Transpose(K);
+
+}
*/