Delta Robot example
Dependencies: BufferedSerial Eigen
Fork of TCPSocket_Example by
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?
User | Revision | Line number | New 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 | } |