Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: solaESKF.hpp
- Revision:
- 80:b241c058df83
- Parent:
- 79:365ea9277167
- Child:
- 81:230a3d2b0493
--- a/solaESKF.hpp Fri Dec 10 10:42:09 2021 +0000 +++ b/solaESKF.hpp Fri Jun 24 05:43:46 2022 +0000 @@ -28,6 +28,7 @@ MatrixXf Phat; MatrixXf Q; + void setDiag(Matrix3f& mat, float val); void setDiag(MatrixXf& mat, float val); void setBlockDiag(MatrixXf& mat, float val,int startIndex, int endIndex); @@ -42,14 +43,16 @@ Vector3f getGyroBias(); Vector3f getGravity(); VectorXf getErrState(); + VectorXf getState(); + VectorXf getVariance(); - void setPhatPosition(float val); - void setPhatVelocity(float val); + void setPhatPosition(float valNE,float valD); + void setPhatVelocity(float valNE,float valD); void setPhatAngleError(float val); void setPhatAccBias(float val); void setPhatGyroBias(float val); void setPhatGravity(float val); - void setQVelocity(float val); + void setQVelocity(float valNE,float valD); void setQAngleError(float val); void setQAccBias(float val); void setQGyroBias(float val); @@ -61,6 +64,8 @@ void updateGPSVelocity(Vector3f velgps, Matrix3f R); void updateAcc(Vector3f acc, Matrix3f R); void updateHeading(float a, Matrix<float, 1, 1> R); + void updateIMU(Vector3f acc,float heading, Matrix<float, 4, 4> R); + void updateWhole(Vector3f posgps,float palt, Vector3f velgps, Vector3f acc,float heading, MatrixXf R); void fuseErr2Nominal(); void computeDcm(Matrix3f& dcm, Vector4f quat);