Delta Robot example
Dependencies: BufferedSerial Eigen
Fork of TCPSocket_Example by
Diff: ESKF.h
- Revision:
- 5:01e1e68309ae
diff -r 778bc352c47f -r 01e1e68309ae ESKF.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ESKF.h Mon Oct 15 18:30:20 2018 +0000 @@ -0,0 +1,78 @@ +#include <Core.h> +#include <Geometry.h> +#include <iostream> + + +#define GRAVITY 9.812 // London g value. +#define SUPPORT_STDIOSTREAM + +using namespace Eigen; +using namespace std; + +//the main ESKF class +class ESKF{ +public: + // takes as input the variance of the acceleration and gyro, where _n is the measurement noise, and _w is the pertibations of the system. + ESKF(Matrix<float, 19,1> initialState, float sig2_a_n_, float sig2_omega_n_,float sig2_a_w_, float sig2_omega_w_); + ESKF(); + // concatonates relevant vectors to one large vector. + Matrix<float, 19,1> makeState(Vector3f p,Vector3f v, Quaternionf q, Vector3f a_b, Vector3f omega_b,Vector3f g ); + + // Called when there is a new measurment from the IMU. + void updateStateIMU(Vector3f a, Vector3f omega, float delta_t); + + // Called when there is a new measurment from an absolute position reference (such as Motion Capture, GPS, map matching etc ) + void observeErrorState(Vector3f pos, Quaternionf rot); +//private: + + Matrix<float, 3,3> getRotationMatrixFromState(Matrix<float, 19,1> state); + Matrix<float,3,3> getSkew(Vector3f in); + Matrix<float,3,3> AngAxToMat(Vector3f in); + Matrix<float,19,1> measurementFunc(Matrix<float,19,1> in); + void composeTrueState(); + void injectObservedError(); + void resetError(); + + void predictionUpdate(Vector3f a, Vector3f omega, float delta_t); + + // states + Matrix<float, 19,1> trueState; + Matrix<float, 18,1> errorState; + Matrix<float, 19,1> nominalState; + + + //covarience matrices as defined on page 59 + Matrix<float, 3,3> V_i; + Matrix<float, 3,3> PHI_i; + Matrix<float, 3,3> A_i; + Matrix<float, 3,3> OMEGA_i; + float sig2_a_n; + float sig2_a_w; + float sig2_omega_n; + float sig2_omega_w; + + + //jacobians of f() as defined on page 59// not sure if should be 19 in size, the quaternion seems to be a rotation matrix here. + Matrix<float, 18,18> F_x; + Matrix<float, 18,12> F_i; + //covariances matrix of the perturbation impulses. + Matrix <float, 12,12> Q_i; + + // the P matrix + Matrix<float, 18,18> P; + + // the K matrix + Matrix<float, 18,19> K; + + + //Jacobian of the true state with respect to the error state. Page 61. + Matrix<float, 19,18> H; + Matrix<float, 19, 19> H_x; + Matrix<float, 19, 18> X_dx; + + Matrix<float, 4, 3> Q_dTheta; + + // Jacobian matrix defined on page 63, can have a simple implementation as an Identity, or one that has a correction term + Matrix<float, 18,18> G; + +};