joshua Elsdon / Mbed OS DeltaRobotFirmware

Delta Robot example

Dependencies:   BufferedSerial Eigen

Fork of TCPSocket_Example by mbed_example

ESKF.cpp@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 <ESKF.h>
je310 5:01e1e68309ae 2
je310 5:01e1e68309ae 3
je310 5:01e1e68309ae 4 ESKF::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 5 trueState = initialState;
je310 5:01e1e68309ae 6 sig2_a_n = sig2_a_n_;
je310 5:01e1e68309ae 7 sig2_omega_n = sig2_omega_n_;
je310 5:01e1e68309ae 8 sig2_a_w = sig2_a_w_;
je310 5:01e1e68309ae 9 sig2_omega_w = sig2_omega_w_;
je310 5:01e1e68309ae 10
je310 5:01e1e68309ae 11 //Build F_i,
je310 5:01e1e68309ae 12 F_i.setZero();
je310 5:01e1e68309ae 13 F_i.block(3,0,12,12).setIdentity();
je310 5:01e1e68309ae 14 }
je310 5:01e1e68309ae 15
je310 5:01e1e68309ae 16 ESKF::ESKF(){
je310 5:01e1e68309ae 17
je310 5:01e1e68309ae 18 }
je310 5:01e1e68309ae 19
je310 5:01e1e68309ae 20
je310 5:01e1e68309ae 21 Matrix<float, 19,1> ESKF::makeState(Vector3f p,Vector3f v, Quaternionf q, Vector3f a_b, Vector3f omega_b,Vector3f g ){
je310 5:01e1e68309ae 22
je310 5:01e1e68309ae 23 }
je310 5:01e1e68309ae 24
je310 5:01e1e68309ae 25 void ESKF::updateStateIMU(Vector3f a, Vector3f omega, float delta_t){
je310 5:01e1e68309ae 26
je310 5:01e1e68309ae 27
je310 5:01e1e68309ae 28 }
je310 5:01e1e68309ae 29
je310 5:01e1e68309ae 30 Matrix<float, 3,3> ESKF::getRotationMatrixFromState(Matrix<float, 19,1> state){
je310 5:01e1e68309ae 31 Matrix<float,4,1> mat = state.block(6,0,4,1);
je310 5:01e1e68309ae 32 Quaternionf quat(mat);
je310 5:01e1e68309ae 33 return quat.matrix();
je310 5:01e1e68309ae 34 }
je310 5:01e1e68309ae 35
je310 5:01e1e68309ae 36 Matrix<float,3,3> ESKF::getSkew(Vector3f in){
je310 5:01e1e68309ae 37 Matrix<float,3,3> out;
je310 5:01e1e68309ae 38 out << 0, -in(2), in(1),
je310 5:01e1e68309ae 39 in(2), 0, -in(0),
je310 5:01e1e68309ae 40 -in(1), in(0), 0;
je310 5:01e1e68309ae 41 return out;
je310 5:01e1e68309ae 42 }
je310 5:01e1e68309ae 43
je310 5:01e1e68309ae 44 Matrix<float,3,3> ESKF::AngAxToMat(Vector3f in){
je310 5:01e1e68309ae 45 float angle = in.norm();
je310 5:01e1e68309ae 46 Vector3f axis = in.normalized();
je310 5:01e1e68309ae 47 if(angle == 0) axis = Vector3f(1,0,0);
je310 5:01e1e68309ae 48
je310 5:01e1e68309ae 49
je310 5:01e1e68309ae 50 AngleAxisf angAx(angle,axis);
je310 5:01e1e68309ae 51 return angAx.toRotationMatrix();
je310 5:01e1e68309ae 52 }
je310 5:01e1e68309ae 53
je310 5:01e1e68309ae 54 void ESKF::predictionUpdate(Vector3f a, Vector3f omega, float delta_t){
je310 5:01e1e68309ae 55 // build F_x
je310 5:01e1e68309ae 56 Matrix<float, 3,3> I3 , I3dt;
je310 5:01e1e68309ae 57 F_x = F_x.Zero(18,18);
je310 5:01e1e68309ae 58 //page 59
je310 5:01e1e68309ae 59 I3 = I3.Identity();
je310 5:01e1e68309ae 60 I3dt = delta_t * I3;
je310 5:01e1e68309ae 61 F_x.block(0,0,3,3) = I3;
je310 5:01e1e68309ae 62 F_x.block(3,3,3,3) = I3;
je310 5:01e1e68309ae 63 F_x.block(9,9,3,3) = I3;
je310 5:01e1e68309ae 64 F_x.block(12,12,3,3) = I3;
je310 5:01e1e68309ae 65 F_x.block(15,15,3,3) = I3;
je310 5:01e1e68309ae 66 F_x.block(0,3,3,3) = I3dt;
je310 5:01e1e68309ae 67 F_x.block(3,15,3,3) = I3dt;
je310 5:01e1e68309ae 68 F_x.block(6,12,3,3) = -I3dt;
je310 5:01e1e68309ae 69
je310 5:01e1e68309ae 70 Matrix<float, 3,3> rotation;
je310 5:01e1e68309ae 71 rotation = getRotationMatrixFromState(nominalState);
je310 5:01e1e68309ae 72 F_x.block(3,9,3,3) = -rotation*delta_t;
je310 5:01e1e68309ae 73
je310 5:01e1e68309ae 74 // for the 2nd row and 3rd column
je310 5:01e1e68309ae 75 F_x.block(3,6,3,3) = - rotation * getSkew(a - nominalState.block(9,0,3,1)) * delta_t;
je310 5:01e1e68309ae 76
je310 5:01e1e68309ae 77 // for the 3rd row 3rd column
je310 5:01e1e68309ae 78 F_x.block(6,6,3,3) = AngAxToMat((omega - nominalState.block(12,0,3,1))*delta_t).transpose();
je310 5:01e1e68309ae 79
je310 5:01e1e68309ae 80
je310 5:01e1e68309ae 81 // build Q_i, this is only a diagonal matrix augmented by a scalar, so could be more efficient to for loop the relevant entries.
je310 5:01e1e68309ae 82
je310 5:01e1e68309ae 83 Q_i.setZero();
je310 5:01e1e68309ae 84 Q_i.block(0,0,3,3) = delta_t * delta_t * sig2_a_n * I3 ;
je310 5:01e1e68309ae 85 Q_i.block(3,3,3,3) = I3 * sig2_omega_n * delta_t * delta_t;
je310 5:01e1e68309ae 86 Q_i.block(6,6,3,3) = I3 * sig2_a_w * delta_t;
je310 5:01e1e68309ae 87 Q_i.block(9,9,3,3) = I3 * sig2_omega_w * delta_t;
je310 5:01e1e68309ae 88
je310 5:01e1e68309ae 89 //probably unnecessary copying here. Need to check if things are done inplace or otherwise. //.eval should fix this issue
je310 5:01e1e68309ae 90 P = (F_x*P*F_x.transpose() + F_i*Q_i*F_i.transpose()).eval();
je310 5:01e1e68309ae 91
je310 5:01e1e68309ae 92
je310 5:01e1e68309ae 93 //this line is apparently not needed, according to the document. // I suspect it only meant in the first iteration?????
je310 5:01e1e68309ae 94 //Matrix<float,18,18> errorState_new = F_x * errorState;
je310 5:01e1e68309ae 95 //errorState = errorState_new;
je310 5:01e1e68309ae 96
je310 5:01e1e68309ae 97
je310 5:01e1e68309ae 98 }
je310 5:01e1e68309ae 99
je310 5:01e1e68309ae 100 Matrix<float,19,1> ESKF::measurementFunc(Matrix<float,19,1> in){
je310 5:01e1e68309ae 101 Matrix<float,19,1> func;
je310 5:01e1e68309ae 102 func << 1,1,1
je310 5:01e1e68309ae 103 ,0,0,0
je310 5:01e1e68309ae 104 ,1,1,1,1
je310 5:01e1e68309ae 105 ,0,0,0
je310 5:01e1e68309ae 106 ,0,0,0
je310 5:01e1e68309ae 107 ,0,0,0;
je310 5:01e1e68309ae 108 return (in.array()*func.array()).matrix();
je310 5:01e1e68309ae 109 }
je310 5:01e1e68309ae 110
je310 5:01e1e68309ae 111 void ESKF::composeTrueState(){
je310 5:01e1e68309ae 112
je310 5:01e1e68309ae 113 }
je310 5:01e1e68309ae 114
je310 5:01e1e68309ae 115
je310 5:01e1e68309ae 116 // this function puts the errorstate into the nominal state. as per page 62
je310 5:01e1e68309ae 117 void ESKF::injectObservedError(){
je310 5:01e1e68309ae 118 Matrix<float,19,1> newState;
je310 5:01e1e68309ae 119 // compose position
je310 5:01e1e68309ae 120 newState.block(0,0,3,1) = nominalState.block(0,0,3,1) + errorState.block(0,0,3,1);
je310 5:01e1e68309ae 121 // compose Velocity
je310 5:01e1e68309ae 122 newState.block(3,0,3,1) = nominalState.block(3,0,3,1) + errorState.block(3,0,3,1);
je310 5:01e1e68309ae 123
je310 5:01e1e68309ae 124 // compose Quaternion - probably this can be done in less lines.
je310 5:01e1e68309ae 125 Matrix<float,3,1> angAxMat = errorState.block(6,0,3,1);
je310 5:01e1e68309ae 126 AngleAxisf AngAx(angAxMat.norm(),angAxMat.normalized());
je310 5:01e1e68309ae 127 Quaternionf qError(AngAx);
je310 5:01e1e68309ae 128 Matrix<float,4,1> qMat = nominalState.block(6,0,4,1);
je310 5:01e1e68309ae 129 Quaternionf qNom(qMat);
je310 5:01e1e68309ae 130 newState.block(6,0,4,1) = (qNom*qError).coeffs();
je310 5:01e1e68309ae 131
je310 5:01e1e68309ae 132 //compose accelerometer drift
je310 5:01e1e68309ae 133 newState.block(10,0,3,1) = nominalState.block(10,0,3,1) + errorState.block(9,0,3,1);
je310 5:01e1e68309ae 134
je310 5:01e1e68309ae 135 //compose gyro drift.
je310 5:01e1e68309ae 136 newState.block(13,0,3,1) = nominalState.block(13,0,3,1) + errorState.block(12,0,3,1);
je310 5:01e1e68309ae 137
je310 5:01e1e68309ae 138 //compose gravity. (I don't think it changes anything.)
je310 5:01e1e68309ae 139 newState.block(16,0,3,1) = nominalState.block(16,0,3,1) + errorState.block(15,0,3,1);
je310 5:01e1e68309ae 140
je310 5:01e1e68309ae 141 //update the nominal state (I am doing a copy to be sure there is no aliasing problems etc)
je310 5:01e1e68309ae 142 nominalState = newState;
je310 5:01e1e68309ae 143 }
je310 5:01e1e68309ae 144
je310 5:01e1e68309ae 145 void ESKF::resetError(){
je310 5:01e1e68309ae 146 // set the errorState to zero
je310 5:01e1e68309ae 147 errorState.Zero();
je310 5:01e1e68309ae 148
je310 5:01e1e68309ae 149 // set up G matrix, can be simply an identity or with a more compicated term for the rotation section.
je310 5:01e1e68309ae 150 G.setIdentity();
je310 5:01e1e68309ae 151 Matrix<float,3,3> rotCorrection;
je310 5:01e1e68309ae 152 rotCorrection = - getSkew(0.5*errorState.block(6,0,3,1));
je310 5:01e1e68309ae 153 G.block(6,6,3,3) = (G.block(6,6,3,3) + rotCorrection).eval();
je310 5:01e1e68309ae 154 P = (G * P * G.transpose()).eval();
je310 5:01e1e68309ae 155
je310 5:01e1e68309ae 156 }
je310 5:01e1e68309ae 157
je310 5:01e1e68309ae 158
je310 5:01e1e68309ae 159 // this function is called when you have a reference to correct the error state, in this case a mocap system.
je310 5:01e1e68309ae 160 void ESKF::observeErrorState(Vector3f pos, Quaternionf rot){
je310 5:01e1e68309ae 161 Matrix<float,19,1> y;
je310 5:01e1e68309ae 162 y.Zero();
je310 5:01e1e68309ae 163 y.block(0,0,3,1) = pos;
je310 5:01e1e68309ae 164 y.block(6,0,4,1) <<rot.x() , rot.y() , rot.z() , rot.w();
je310 5:01e1e68309ae 165
je310 5:01e1e68309ae 166 // setup X_dx, essensially an identity, with some quaternion stuff in the middle. Optimise by initilising everything elsewhere.
je310 5:01e1e68309ae 167 X_dx.Zero();
je310 5:01e1e68309ae 168 Matrix<float, 6,6> I6;
je310 5:01e1e68309ae 169 I6 = I6.Identity();
je310 5:01e1e68309ae 170 Matrix<float,9,9> I9;
je310 5:01e1e68309ae 171 I9 = I9.Identity();
je310 5:01e1e68309ae 172 X_dx.block(0,0,6,6) = I6;
je310 5:01e1e68309ae 173 X_dx.block(10,9,9,9) = I9;
je310 5:01e1e68309ae 174 Matrix<float,4,1> q(nominalState.block(6,0,4,1)); // getting quaternion, though in a mat, so we can divide by 2.
je310 5:01e1e68309ae 175 q = q/2;
je310 5:01e1e68309ae 176 X_dx.block(6,6,4,3) << -q.x() , -q.y() , -q.z(),
je310 5:01e1e68309ae 177 q.w() , -q.z() , q.y(),
je310 5:01e1e68309ae 178 q.z() , q.w() , -q.x(),
je310 5:01e1e68309ae 179 -q.y() , q.x() , q.w();
je310 5:01e1e68309ae 180
je310 5:01e1e68309ae 181 // then set up H_x, though this is not told to us directly, it is described as:
je310 5:01e1e68309ae 182 /*"Here, Hx , ∂h∂xt|x
je310 5:01e1e68309ae 183 is the standard Jacobian of h() with respect to its own argument (i.e.,
je310 5:01e1e68309ae 184 the Jacobian one would use in a regular EKF). This first part of the chain rule depends on
je310 5:01e1e68309ae 185 the measurement function of the particular sensor used, and is not presented here.*/
je310 5:01e1e68309ae 186
je310 5:01e1e68309ae 187 //I believe that if I am only providing position and a quaternion then the position part will be an identity, the quaternion part will be identity?
je310 5:01e1e68309ae 188
je310 5:01e1e68309ae 189 H_x = H_x.Identity();
je310 5:01e1e68309ae 190
je310 5:01e1e68309ae 191 //compose the two halves of the hessian
je310 5:01e1e68309ae 192 H = H_x*X_dx;
je310 5:01e1e68309ae 193 Matrix<float,19,19> V; // the covariance of the measurement function.
je310 5:01e1e68309ae 194 V.Zero();
je310 5:01e1e68309ae 195 K = P * H.transpose() * (H*P*H.transpose() + V).inverse();
je310 5:01e1e68309ae 196
je310 5:01e1e68309ae 197 Matrix<float,18,1> d_x_hat;
je310 5:01e1e68309ae 198 composeTrueState();
je310 5:01e1e68309ae 199 d_x_hat = K *(y - measurementFunc(trueState));
je310 5:01e1e68309ae 200 Matrix<float,18,18> I18;
je310 5:01e1e68309ae 201 I18 = I18.Identity();
je310 5:01e1e68309ae 202 P = ((I18 - K*H)*P).eval();
je310 5:01e1e68309ae 203
je310 5:01e1e68309ae 204 injectObservedError();
je310 5:01e1e68309ae 205
je310 5:01e1e68309ae 206 resetError();
je310 5:01e1e68309ae 207
je310 5:01e1e68309ae 208
je310 5:01e1e68309ae 209
je310 5:01e1e68309ae 210
je310 5:01e1e68309ae 211 }