HAPSRG
Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: ScErrStateEKF.cpp
- Revision:
- 31:e655d4d8e4d6
- Parent:
- 30:ff884e9b2e30
- Child:
- 32:321a756e12ad
--- a/ScErrStateEKF.cpp Wed Sep 22 09:30:37 2021 +0000
+++ b/ScErrStateEKF.cpp Tue Sep 28 08:17:42 2021 +0000
@@ -8,20 +8,21 @@
using namespace std;
ScErrStateEKF::ScErrStateEKF()
- :qhat(4,1),vihat(3,1), errState(12,1), Phat(12,12), Q(12,12), Ra(3,3), Rgps(3,3),Rsr(1,1), 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(9,1), bgState(3,1), Phat(9,9), Q(9,9),Phatbg(3,3),Qbg(3,3), Ra(3,3), Rgps(3,3),Rsr(1,1), 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;
setDiag(Phat,0.1f);
+ setDiag(Phatbg,0.1f);
setQqerr(0.000005f);
setQgbias(0.00001f);
- setQabias(1.0f);
+ setQabias(0.00001f);
setQv(0.000001f);
//加速度の観測
setDiag(Ra,0.005f);
- setDiag(Qab,100.0f);
+ setDiag(Qab,10000.0f);
//ジャイロバイアスに関する制約
setDiag(Rgsc,500.0f);
@@ -31,8 +32,8 @@
setDiag(Rm,5.0f);
- setDiag(Rgps,0.01f);
- setDiag(Rsr,10.0f);
+ setDiag(Rgps,5.5f);
+ setDiag(Rsr,5.5f);
for(int i = 0; i<10;i++){
histffunc[i] = 0.0f;
@@ -74,9 +75,6 @@
A(2,3) = gyro.x;
A(3,1) = gyro.y;
A(3,2) = -gyro.x;
- A(1,4) = -0.5f;
- A(2,5) = -0.5f;
- A(3,6) = -0.5f;
Matrix dcm(3,3);
computeDcm(dcm, qhat);
@@ -84,8 +82,8 @@
Matrix baterm = -9.8f*MatrixMath::Transpose(dcm);
for (int i = 1; i < 4; i++){
for (int j = 1; i < 4; i++){
- A(i+9,j) = qeterm(i,j);
- A(i+9,j+6) = baterm(i,j);
+ A(i+6,j) = qeterm(i,j);
+ A(i+6,j+3) = baterm(i,j);
}
}
@@ -101,20 +99,20 @@
}
void ScErrStateEKF::setQgbias(float val){
+ Qbg(1,1) = val;
+ Qbg(2,2) = val;
+ Qbg(2,2) = val;
+}
+void ScErrStateEKF::setQabias(float val){
Q(4,4) = val;
Q(5,5) = val;
Q(6,6) = val;
}
-void ScErrStateEKF::setQabias(float val){
+void ScErrStateEKF::setQv(float val){
Q(7,7) = val;
Q(8,8) = val;
Q(9,9) = val;
}
-void ScErrStateEKF::setQv(float val){
- Q(10,10) = val;
- Q(11,11) = val;
- Q(12,12) = val;
-}
void ScErrStateEKF::setQab(float val){
setDiag(Qab,val);
@@ -148,12 +146,12 @@
resVal[0] = gyroBias.x;
resVal[1] = gyroBias.y;
resVal[2] = gyroBias.z;
- resCov[0] = Phat(4,4);
- resCov[1] = Phat(4,5);
- resCov[2] = Phat(4,6);
- resCov[3] = Phat(5,5);
- resCov[4] = Phat(5,6);
- resCov[5] = Phat(6,6);
+ resCov[0] = Phatbg(1,1);
+ resCov[1] = Phatbg(1,2);
+ resCov[2] = Phatbg(1,3);
+ resCov[3] = Phatbg(2,2);
+ resCov[4] = Phatbg(2,3);
+ resCov[5] = Phatbg(3,3);
}
@@ -161,12 +159,12 @@
resVal[0] = accBias.x;
resVal[1] = accBias.y;
resVal[2] = accBias.z;
- resCov[0] = Phat(7,7);
- resCov[1] = Phat(7,8);
- resCov[2] = Phat(7,9);
- resCov[3] = Phat(8,8);
- resCov[4] = Phat(8,9);
- resCov[5] = Phat(9,9);
+ resCov[0] = Phat(4,4);
+ resCov[1] = Phat(4,5);
+ resCov[2] = Phat(4,6);
+ resCov[3] = Phat(5,5);
+ resCov[4] = Phat(5,6);
+ resCov[5] = Phat(6,6);
}
@@ -184,9 +182,9 @@
H(2,3) = -2.0f*gvec.x;
H(3,1) = -2.0f*gvec.y;
H(3,2) = 2.0f*gvec.x;
- H(1,7) = 1.0f;
- H(2,8) = 1.0f;
- H(3,9) = 1.0f;
+ H(1,4) = 1.0f;
+ H(2,5) = 1.0f;
+ H(3,6) = 1.0f;
Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Ra+Qab);
Matrix z = MatrixMath::Vector2mat(acc)-dcm*MatrixMath::Vector2mat(accref);
Matrix corrVal = K * (z-H*errState);
@@ -197,161 +195,25 @@
void ScErrStateEKF::updateGyroBiasConstraints(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(2,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 Pm(nState,nState);
- for(int i = 4; i<7; i++){
- for(int j = 4;j<7;j++){
- Pm(i,j) = Phat(i,j);
- }
- }
-
- Matrix K = (Pm*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Pm*MatrixMath::Transpose(H)+Rgsc);
- Matrix z(2,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;
- Matrix corrVal = K * (z-H*errState);
- errState = errState + corrVal;
- Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Rgsc)*MatrixMath::Transpose(K);
-}
-
-
-void ScErrStateEKF::updateMeasures(Vector3 gyro, Vector3 acc, Vector3 accref)
-{
- acc -= accBias;
- gyro -= gyroBias;
Matrix dcm(3,3);
computeDcm(dcm, qhat);
- Vector3 gvec(dcm(1,3)*accref.z, dcm(2,3)*accref.z, dcm(3,3)*accref.z);
- Matrix H(5,nState);
- H(1,2) = -2.0f*gvec.z;
- H(1,3) = 2.0f*gvec.y;
- H(2,1) = 2.0f*gvec.z;
- H(2,3) = -2.0f*gvec.x;
- H(3,1) = -2.0f*gvec.y;
- H(3,2) = 2.0f*gvec.x;
- H(1,7) = 1.0f;
- H(2,8) = 1.0f;
- H(3,9) = 1.0f;
- /*
- Matrix qeterm = -2.0f*MatrixMath::Transpose(dcm)*MatrixMath::Matrixcross(gyro.x,gyro.y,gyro.z);
- for(int j = 1;j<4;j++){
- H(4,j) = qeterm(1,j);
- H(5,j) = qeterm(2,j);
- }
- H(4,4) = 1.0f*(dcm(1,1));
- H(4,5) = 1.0f*(dcm(2,1));
- H(4,6) = 1.0f*(dcm(3,1));
- H(5,4) = 1.0f*(dcm(1,2));
- H(5,5) = 1.0f*(dcm(2,2));
- H(5,6) = 1.0f*(dcm(3,2));
- */
- 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(4,j) = -qeterm(1,j);
- H(5,j) = -qeterm(3,j);
- //H(3,j) = -qeterm(3,j);
- H(4,9+j) = -dcm(1,j);
- H(5,9+j) = -dcm(3,j);
- //H(3,9+j) = -dcm(3,j);
- }
+ Matrix H(2,3);
+ H(1,1) = 1.0f*(dcm(1,1));
+ H(1,2) = 1.0f*(dcm(2,1));
+ H(1,3) = 1.0f*(dcm(3,1));
+ H(2,1) = 1.0f*(dcm(1,2));
+ H(2,2) = 1.0f*(dcm(2,2));
+ H(2,3) = 1.0f*(dcm(3,2));
- Matrix R(5,5);
- R(1,1) = Ra(1,1)+Qab(1,1);
- R(2,2) = Ra(2,2)+Qab(2,2);
- R(3,3) = Ra(3,3)+Qab(3,3);
- //R(4,4) = Rgsc(1,1);
- //R(5,5) = Rgsc(2,2);
- R(4,4) = Rvsc(1,1);
- R(5,5) = Rvsc(2,2);
-
- Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
- Matrix zacc = MatrixMath::Vector2mat(acc)-dcm*MatrixMath::Vector2mat(accref);
- Matrix z(5,1);
- z << zacc(1,1)<< zacc(2,1)<< zacc(3,1) << 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);
+ Matrix K = (Phatbg*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phatbg*MatrixMath::Transpose(H)+Rgsc);
+ Matrix z(2,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;
+ Matrix corrVal = K * (z-H*bgState);
+ bgState = bgState + corrVal;
+ Phatbg = (MatrixMath::Eye(3)-K*H)*Phatbg*MatrixMath::Transpose(MatrixMath::Eye(3)-K*H)+K*(Rgsc)*MatrixMath::Transpose(K);
}
-void ScErrStateEKF::updateStaticMeasures(Vector3 gyro,Vector3 acc, Vector3 accref)
-{
- acc -= accBias;
- gyro -= gyroBias;
- Matrix dcm(3,3);
- computeDcm(dcm, qhat);
- Vector3 gvec(dcm(1,3)*accref.z, dcm(2,3)*accref.z, dcm(3,3)*accref.z);
- Matrix H(5,nState);
- H(1,2) = -2.0f*gvec.z;
- H(1,3) = 2.0f*gvec.y;
- H(2,1) = 2.0f*gvec.z;
- H(2,3) = -2.0f*gvec.x;
- H(3,1) = -2.0f*gvec.y;
- H(3,2) = 2.0f*gvec.x;
- H(1,7) = 1.0f;
- H(2,8) = 1.0f;
- H(3,9) = 1.0f;
- /*
- Matrix qeterm = -2.0f*MatrixMath::Transpose(dcm)*MatrixMath::Matrixcross(gyro.x,gyro.y,gyro.z);
- for(int j = 1;j<4;j++){
- H(4,j) = qeterm(1,j);
- H(5,j) = qeterm(2,j);
- }
- H(4,4) = 1.0f*(dcm(1,1));
- H(4,5) = 1.0f*(dcm(2,1));
- H(4,6) = 1.0f*(dcm(3,1));
- H(5,4) = 1.0f*(dcm(1,2));
- H(5,5) = 1.0f*(dcm(2,2));
- H(5,6) = 1.0f*(dcm(3,2));
- */
- 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(4,j) = -qeterm(1,j);
- H(5,j) = -qeterm(3,j);
- //H(3,j) = -qeterm(3,j);
- H(4,9+j) = -dcm(1,j);
- H(5,9+j) = -dcm(3,j);
- //H(3,9+j) = -dcm(3,j);
- }
-
- Matrix R(5,5);
- R(1,1) = Ra(1,1)*100.0f;
- R(2,2) = Ra(2,2)*100.0f;
- R(3,3) = Ra(3,3)*100.0f;
- //R(4,4) = Rgsc(1,1);
- //R(5,5) = Rgsc(2,2);
- R(4,4) = Rvsc(1,1);
- R(5,5) = Rvsc(2,2);
-
- Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
- Matrix zacc = MatrixMath::Vector2mat(acc)-dcm*MatrixMath::Vector2mat(accref);
- Matrix z(5,1);
- z << zacc(1,1)<< zacc(2,1)<< zacc(3,1) << 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);
-}
-
-
-
-
void ScErrStateEKF::updateStaticAccMeasures(Vector3 acc, Vector3 accref)
{
Matrix dcm(3,3);
@@ -374,30 +236,72 @@
Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Ra*100.0f)*MatrixMath::Transpose(K);
}
-void ScErrStateEKF::updateGPSVelocity(float vi_x, float vi_y, float sinkRate)
+void ScErrStateEKF::updateGPSVelocity(float vi_x, float vi_y,Vector3 acc, Vector3 accref)
{
- Matrix H(3,nState);
- H(1,10) = 1.0f;
- H(2,11) = 1.0f;
- H(3,12) = 1.0f;
- Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Rgps);
- Matrix z(3,1);
- z << vi_x - vihat(1,1) << vi_y-vihat(2,1) << sinkRate - vihat(3,1);
+ acc -= accBias;
+ Matrix dcm(3,3);
+ computeDcm(dcm, qhat);
+ Vector3 gvec(dcm(1,3)*accref.z, dcm(2,3)*accref.z, dcm(3,3)*accref.z);
+ Matrix H(5,nState);
+ H(1,2) = -2.0f*gvec.z;
+ H(1,3) = 2.0f*gvec.y;
+ H(2,1) = 2.0f*gvec.z;
+ H(2,3) = -2.0f*gvec.x;
+ H(3,1) = -2.0f*gvec.y;
+ H(3,2) = 2.0f*gvec.x;
+ H(1,4) = 1.0f;
+ H(2,5) = 1.0f;
+ H(3,6) = 1.0f;
+ H(4,7) = 1.0f;
+ H(5,8) = 1.0f;
+
+ Matrix R(5,5);
+ R(1,1) = Ra(1,1)+Qab(1,1);
+ R(2,2) = Ra(2,2)+Qab(2,2);
+ R(3,3) = Ra(3,3)+Qab(3,3);
+ R(4,4) = Rgps(1,1);
+ R(5,5) = Rgps(2,2);
+
+ Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
+ Matrix zacc = MatrixMath::Vector2mat(acc)-dcm*MatrixMath::Vector2mat(accref);
+ Matrix z(5,1);
+ z << zacc(1,1)<< zacc(2,1)<< zacc(3,1) << vi_x - vihat(1,1) << vi_y-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);
+ Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(R)*MatrixMath::Transpose(K);
}
-void ScErrStateEKF::updateSinkRate(float sinkRate)
+void ScErrStateEKF::updateSinkRate(float sinkRate,Vector3 acc, Vector3 accref)
{
- Matrix H(1,nState);
- H(1,12) = 1.0f;
- Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Rsr);
- Matrix z(1,1);
- z << sinkRate - vihat(3,1);
+ acc -= accBias;
+ Matrix dcm(3,3);
+ computeDcm(dcm, qhat);
+ Vector3 gvec(dcm(1,3)*accref.z, dcm(2,3)*accref.z, dcm(3,3)*accref.z);
+ Matrix H(4,nState);
+ H(1,2) = -2.0f*gvec.z;
+ H(1,3) = 2.0f*gvec.y;
+ H(2,1) = 2.0f*gvec.z;
+ H(2,3) = -2.0f*gvec.x;
+ H(3,1) = -2.0f*gvec.y;
+ H(3,2) = 2.0f*gvec.x;
+ H(1,4) = 1.0f;
+ H(2,5) = 1.0f;
+ H(3,6) = 1.0f;
+ H(4,9) = 1.0f;
+
+ Matrix R(4,4);
+ R(1,1) = Ra(1,1)+Qab(1,1);
+ R(2,2) = Ra(2,2)+Qab(2,2);
+ R(3,3) = Ra(3,3)+Qab(3,3);
+ R(4,4) = Rsr(1,1);
+
+ Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
+ Matrix zacc = MatrixMath::Vector2mat(acc)-dcm*MatrixMath::Vector2mat(accref);
+ Matrix z(4,1);
+ z << zacc(1,1)<< zacc(2,1)<< zacc(3,1) << sinkRate - vihat(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*(Rsr)*MatrixMath::Transpose(K);
+ Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(R)*MatrixMath::Transpose(K);
}
void ScErrStateEKF::updateCenteredGyroBiasCorrection(float (&cBg)[3])
@@ -408,60 +312,34 @@
RgyroBias(i,i) = 0.1f;
}
- Matrix H(3,nState);
- H(1,4) = 1.0f;
- H(2,5) = 1.0f;
- H(3,6) = 1.0f;
+ Matrix H(3,3);
+ H(1,1) = 1.0f;
+ H(2,2) = 1.0f;
+ H(3,3) = 1.0f;
- Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+RgyroBias);
+ Matrix K = (Phatbg*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phatbg*MatrixMath::Transpose(H)+RgyroBias);
Matrix z(3,1);
z <<cBg[0]-gyroBias.x <<cBg[1]-gyroBias.y <<cBg[2]-gyroBias.z;
- Matrix corrVal = K * (z-H*errState);
- errState = errState + corrVal;
- Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(RgyroBias)*MatrixMath::Transpose(K);
+ Matrix corrVal = K * (z-H*bgState);
+ bgState = bgState + corrVal;
+ Phatbg = (MatrixMath::Eye(3)-K*H)*Phatbg*MatrixMath::Transpose(MatrixMath::Eye(3)-K*H)+K*(RgyroBias)*MatrixMath::Transpose(K);
}
-void ScErrStateEKF::updateCenteredAccBiasCorrection(float (&cBa)[3])
-{
-
- Matrix RaccBias(3,3);
- for(int i = 1;i<4;i++){
- RaccBias(i,i) = 5.0f;
- }
-
- Matrix H(3,nState);
- H(1,7) = 1.0f;
- H(2,8) = 1.0f;
- H(3,9) = 1.0f;
-
- Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+RaccBias);
- Matrix z(3,1);
- z <<cBa[0]-accBias.x <<cBa[1]-accBias.y <<cBa[2]-accBias.z;
- Matrix corrVal = K * (z-H*errState);
- errState = errState + corrVal;
- Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(RaccBias)*MatrixMath::Transpose(K);
-
- //gyroBias.x = cBg[0];
- //gyroBias.y = cBg[1];
- //gyroBias.z = cBg[2];
-}
-
-
void ScErrStateEKF::resetBias()
{
- gyroBias.x = gyroBias.x + errState(4,1)*1.0f;
- gyroBias.y = gyroBias.y + errState(5,1)*1.0f;
- gyroBias.z = gyroBias.z + errState(6,1)*1.0f;
+ gyroBias.x = gyroBias.x + bgState(1,1)*1.0f;
+ gyroBias.y = gyroBias.y + bgState(2,1)*1.0f;
+ gyroBias.z = gyroBias.z + bgState(3,1)*1.0f;
+ bgState(1,1) = 0.0f;
+ bgState(2,1) = 0.0f;
+ bgState(3,1) = 0.0f;
+ accBias.x = accBias.x + errState(4,1);
+ accBias.y = accBias.y + errState(5,1);
+ accBias.z = accBias.z + errState(6,1);
errState(4,1) = 0.0f;
errState(5,1) = 0.0f;
errState(6,1) = 0.0f;
- accBias.x = accBias.x + errState(7,1);
- accBias.y = accBias.y + errState(8,1);
- accBias.z = accBias.z + errState(9,1);
- errState(7,1) = 0.0f;
- errState(8,1) = 0.0f;
- errState(9,1) = 0.0f;
}
void ScErrStateEKF::computeAngles(Vector3& rpy,Vector3 rpy_align)
@@ -497,12 +375,12 @@
float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
qhat *= (1.0f/ qnorm);
- vihat(1,1) += errState(10,1);
- vihat(2,1) += errState(11,1);
- vihat(3,1) += errState(12,1);
- errState(10,1) = 0.0f;
- errState(11,1) = 0.0f;
- errState(12,1) = 0.0f;
+ vihat(1,1) += errState(7,1);
+ vihat(2,1) += errState(8,1);
+ vihat(3,1) += errState(9,1);
+ errState(7,1) = 0.0f;
+ errState(8,1) = 0.0f;
+ errState(9,1) = 0.0f;
}
Matrix ScErrStateEKF::quatmultiply(Matrix q, Matrix r)