joshua Elsdon / Mbed OS DeltaRobotFirmware

Delta Robot example

Dependencies:   BufferedSerial Eigen

Fork of TCPSocket_Example by mbed_example

ESKF.h@5:01e1e68309ae, 2018-10-15 (annotated)

Committer:
je310
Date:
Mon Oct 15 18:30:20 2018 +0000
Revision:
5:01e1e68309ae
testing eigen;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
je310 5:01e1e68309ae 1 #include <Core.h>
je310 5:01e1e68309ae 2 #include <Geometry.h>
je310 5:01e1e68309ae 3 #include <iostream>
je310 5:01e1e68309ae 4
je310 5:01e1e68309ae 5
je310 5:01e1e68309ae 6 #define GRAVITY 9.812 // London g value.
je310 5:01e1e68309ae 7 #define SUPPORT_STDIOSTREAM
je310 5:01e1e68309ae 8
je310 5:01e1e68309ae 9 using namespace Eigen;
je310 5:01e1e68309ae 10 using namespace std;
je310 5:01e1e68309ae 11
je310 5:01e1e68309ae 12 //the main ESKF class
je310 5:01e1e68309ae 13 class ESKF{
je310 5:01e1e68309ae 14 public:
je310 5:01e1e68309ae 15 // takes as input the variance of the acceleration and gyro, where _n is the measurement noise, and _w is the pertibations of the system.
je310 5:01e1e68309ae 16 ESKF(Matrix<float, 19,1> initialState, float sig2_a_n_, float sig2_omega_n_,float sig2_a_w_, float sig2_omega_w_);
je310 5:01e1e68309ae 17 ESKF();
je310 5:01e1e68309ae 18 // concatonates relevant vectors to one large vector.
je310 5:01e1e68309ae 19 Matrix<float, 19,1> makeState(Vector3f p,Vector3f v, Quaternionf q, Vector3f a_b, Vector3f omega_b,Vector3f g );
je310 5:01e1e68309ae 20
je310 5:01e1e68309ae 21 // Called when there is a new measurment from the IMU.
je310 5:01e1e68309ae 22 void updateStateIMU(Vector3f a, Vector3f omega, float delta_t);
je310 5:01e1e68309ae 23
je310 5:01e1e68309ae 24 // Called when there is a new measurment from an absolute position reference (such as Motion Capture, GPS, map matching etc )
je310 5:01e1e68309ae 25 void observeErrorState(Vector3f pos, Quaternionf rot);
je310 5:01e1e68309ae 26 //private:
je310 5:01e1e68309ae 27
je310 5:01e1e68309ae 28 Matrix<float, 3,3> getRotationMatrixFromState(Matrix<float, 19,1> state);
je310 5:01e1e68309ae 29 Matrix<float,3,3> getSkew(Vector3f in);
je310 5:01e1e68309ae 30 Matrix<float,3,3> AngAxToMat(Vector3f in);
je310 5:01e1e68309ae 31 Matrix<float,19,1> measurementFunc(Matrix<float,19,1> in);
je310 5:01e1e68309ae 32 void composeTrueState();
je310 5:01e1e68309ae 33 void injectObservedError();
je310 5:01e1e68309ae 34 void resetError();
je310 5:01e1e68309ae 35
je310 5:01e1e68309ae 36 void predictionUpdate(Vector3f a, Vector3f omega, float delta_t);
je310 5:01e1e68309ae 37
je310 5:01e1e68309ae 38 // states
je310 5:01e1e68309ae 39 Matrix<float, 19,1> trueState;
je310 5:01e1e68309ae 40 Matrix<float, 18,1> errorState;
je310 5:01e1e68309ae 41 Matrix<float, 19,1> nominalState;
je310 5:01e1e68309ae 42
je310 5:01e1e68309ae 43
je310 5:01e1e68309ae 44 //covarience matrices as defined on page 59
je310 5:01e1e68309ae 45 Matrix<float, 3,3> V_i;
je310 5:01e1e68309ae 46 Matrix<float, 3,3> PHI_i;
je310 5:01e1e68309ae 47 Matrix<float, 3,3> A_i;
je310 5:01e1e68309ae 48 Matrix<float, 3,3> OMEGA_i;
je310 5:01e1e68309ae 49 float sig2_a_n;
je310 5:01e1e68309ae 50 float sig2_a_w;
je310 5:01e1e68309ae 51 float sig2_omega_n;
je310 5:01e1e68309ae 52 float sig2_omega_w;
je310 5:01e1e68309ae 53
je310 5:01e1e68309ae 54
je310 5:01e1e68309ae 55 //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.
je310 5:01e1e68309ae 56 Matrix<float, 18,18> F_x;
je310 5:01e1e68309ae 57 Matrix<float, 18,12> F_i;
je310 5:01e1e68309ae 58 //covariances matrix of the perturbation impulses.
je310 5:01e1e68309ae 59 Matrix <float, 12,12> Q_i;
je310 5:01e1e68309ae 60
je310 5:01e1e68309ae 61 // the P matrix
je310 5:01e1e68309ae 62 Matrix<float, 18,18> P;
je310 5:01e1e68309ae 63
je310 5:01e1e68309ae 64 // the K matrix
je310 5:01e1e68309ae 65 Matrix<float, 18,19> K;
je310 5:01e1e68309ae 66
je310 5:01e1e68309ae 67
je310 5:01e1e68309ae 68 //Jacobian of the true state with respect to the error state. Page 61.
je310 5:01e1e68309ae 69 Matrix<float, 19,18> H;
je310 5:01e1e68309ae 70 Matrix<float, 19, 19> H_x;
je310 5:01e1e68309ae 71 Matrix<float, 19, 18> X_dx;
je310 5:01e1e68309ae 72
je310 5:01e1e68309ae 73 Matrix<float, 4, 3> Q_dTheta;
je310 5:01e1e68309ae 74
je310 5:01e1e68309ae 75 // Jacobian matrix defined on page 63, can have a simple implementation as an Identity, or one that has a correction term
je310 5:01e1e68309ae 76 Matrix<float, 18,18> G;
je310 5:01e1e68309ae 77
je310 5:01e1e68309ae 78 };