HAPSRG
Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: ScErrStateEKF.cpp
- Revision:
- 42:b90c348a49c0
- Parent:
- 40:119792aa6d3b
--- a/ScErrStateEKF.cpp Wed Oct 20 01:50:36 2021 +0000
+++ b/ScErrStateEKF.cpp Thu Oct 21 06:40:38 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), 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), Phat(9,9), Q(9,9), 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;
@@ -22,12 +22,8 @@
Phat(4,4) = 0.001;
Phat(5,5) = 0.001;
Phat(6,6) = 0.001;
- Phat(7,7) = 1.0;
- Phat(8,8) = 1.0;
- Phat(9,9) = 1.0;
setQqerr(0.001f);
setQgbias(0.0001f);
- setQabias(1.0f);
setQv(0.01f);
//加速度の観測
@@ -57,7 +53,7 @@
void ScErrStateEKF::updateNominal(Vector3 gyro, Vector3 acc, Vector3 accref, float att_dt)
{
gyro -= gyroBias;
- acc -= accBias;
+ //acc -= accBias;
Matrix A(4,4);
A << 0.0f << -0.5f*gyro.x <<-0.5f*gyro.y <<-0.5f*gyro.z
<< 0.5f*gyro.x << 0.0f << 0.5f*gyro.z <<-0.5f*gyro.y
@@ -77,7 +73,7 @@
void ScErrStateEKF::updateErrState(Vector3 gyro,Vector3 acc, float att_dt)
{
gyro -= gyroBias;
- acc -= accBias;
+ //acc -= accBias;
Matrix A(nState,nState);
A(1,2) = gyro.z;
A(1,3) = -gyro.y;
@@ -91,12 +87,12 @@
Matrix dcm(3,3);
computeDcm(dcm, qhat);
- Matrix qeterm = -2.0f*9.8f*MatrixMath::Transpose(dcm)*MatrixMath::Matrixcross(acc.x,acc.y,acc.z);
- Matrix baterm = -9.8f*MatrixMath::Transpose(dcm);
+ Matrix qeterm = 2.0f*9.8f*MatrixMath::Transpose(dcm)*MatrixMath::Matrixcross(acc.x,acc.y,acc.z);
+ //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+9,j+6) = baterm(i,j);
}
}
@@ -116,16 +112,12 @@
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);
@@ -168,23 +160,8 @@
}
-void ScErrStateEKF::getAccBias(float (&resVal)[3], float (&resCov)[6]){
- 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);
-
-}
-
-
void ScErrStateEKF::updateAccMeasures(Vector3 acc, Vector3 accref)
{
- 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);
@@ -195,9 +172,6 @@
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 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);
@@ -251,7 +225,6 @@
void ScErrStateEKF::updateGPSVelocity(float vi_x, float vi_y, float sinkRate,Vector3 acc, Vector3 accref)
{
- 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);
@@ -263,12 +236,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(4,10) = 1.0f;
- H(5,11) = 1.0f;
- H(6,12) = 1.0f;
+ H(4,7) = 1.0f;
+ H(5,8) = 1.0f;
+ H(6,9) = 1.0f;
Matrix R(6,6);
R(1,1) = Ra(1,1)+Qab(1,1);
@@ -278,23 +248,10 @@
R(5,5) = Rgps(2,2);
R(6,6) = Rsr(1,1);
- /*
- Matrix H(3, nState);
- H(1, 10) = 1.0f;
- H(2, 11) = 1.0f;
- H(3, 12) = 1.0f;
-
- Matrix R(3,3);
- R(1,1) = Rgps(1,1);
- R(2,2) = Rgps(2,2);
- R(3,3) = 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(6,1);
- z << zacc(1,1)<< zacc(2,1)<< zacc(3,1) << vi_x - vihat(1,1) << vi_y-vihat(2,1) << sinkRate - vihat(3,1);
- //z << vi_x - vihat(1,1) << vi_y-vihat(2,1) << sinkRate - vihat(3,1);
+ z << zacc(1,1)<< zacc(2,1)<< zacc(3,1) << vi_x - vihat(1,1) << vi_y-vihat(2,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*(R)*MatrixMath::Transpose(K);
@@ -302,7 +259,6 @@
void ScErrStateEKF::updateSinkRate(float sinkRate,Vector3 acc, Vector3 accref)
{
- 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);
@@ -313,10 +269,7 @@
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(4,12) = 1.0f;
+ H(4,9) = 1.0f;
Matrix R(4,4);
R(1,1) = Ra(1,1)+Qab(1,1);
@@ -367,12 +320,6 @@
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)
@@ -408,12 +355,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)