ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

Asservissement/Asserv.h@44:d5f95af61243, 2015-03-20 (annotated)

Committer:
Near32
Date:
Fri Mar 20 12:12:07 2015 +0000
Revision:
44:d5f95af61243
Parent:
43:87bdce65341f
Child:
47:4909e97570f6
pid acc pid vit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Near32 36:54f86bc6fd80 1 /*KalmanFilter*/
Near32 36:54f86bc6fd80 2 #include "EKF.h"
Near32 36:54f86bc6fd80 3 #include "Odometry.h"
Near32 43:87bdce65341f 4 /*
Near32 36:54f86bc6fd80 5 template<typename T>
Near32 36:54f86bc6fd80 6 Mat<T> motion_bicycle3( Mat<T> state, Mat<T> command, T dt = (T)0.5);
Near32 36:54f86bc6fd80 7 template<typename T>
Near32 36:54f86bc6fd80 8 Mat<T> sensor_bicycle3( Mat<T> state, Mat<T> command, Mat<T> d_state, T dt = 0.5 );
Near32 36:54f86bc6fd80 9 template<typename T>
Near32 36:54f86bc6fd80 10 Mat<T> jmotion_bicycle3( Mat<T> state, Mat<T> command, T dt = 0.5);
Near32 36:54f86bc6fd80 11 template<typename T>
Near32 36:54f86bc6fd80 12 Mat<T> jmotion_bicycle3_command(Mat<T> state, Mat<T> command, T dt = 0.5);
Near32 36:54f86bc6fd80 13 template<typename T>
Near32 36:54f86bc6fd80 14 Mat<T> jsensor_bicycle3( Mat<T> state, Mat<T> command, Mat<T> d_state, T dt = 0.5);
Near32 36:54f86bc6fd80 15 template<typename T>
Near32 36:54f86bc6fd80 16 bool setPWM(PwmOut *servo,T p);
Near32 43:87bdce65341f 17 */
Near32 36:54f86bc6fd80 18
Near32 36:54f86bc6fd80 19 template<typename T>
Near32 36:54f86bc6fd80 20 bool setPWM(PwmOut *servo,T p)
Near32 36:54f86bc6fd80 21 {
Near32 36:54f86bc6fd80 22 if(p <= (T)1.0 && p >= (T)0.0)
Near32 36:54f86bc6fd80 23 {
Near32 36:54f86bc6fd80 24 servo->write((float)p);
Near32 36:54f86bc6fd80 25 return true;
Near32 36:54f86bc6fd80 26 }
Near32 36:54f86bc6fd80 27
Near32 36:54f86bc6fd80 28 return false;
Near32 36:54f86bc6fd80 29 }
Near32 36:54f86bc6fd80 30
Near32 36:54f86bc6fd80 31 template<typename T>
Near32 36:54f86bc6fd80 32 Mat<T> motion_bicycle3( Mat<T> state, Mat<T> command, T dt)
Near32 43:87bdce65341f 33 {
Near32 36:54f86bc6fd80 34 Mat<T> bicycle(3,1);
Near32 36:54f86bc6fd80 35 bicycle.set((T)36, 1,1); /*radius*/
Near32 36:54f86bc6fd80 36 bicycle.set((T)36, 2,1);
Near32 36:54f86bc6fd80 37 bicycle.set((T)220, 3,1); /*entre-roue*/
Near32 36:54f86bc6fd80 38 Mat<T> r(state);
Near32 36:54f86bc6fd80 39 //double v = bicycle.get(1,1)/(2*bicycle.get(3,1))*(r.get(4,1)+r.get(5,1));
Near32 36:54f86bc6fd80 40 //double w = bicycle.get(1,1)/(2*bicycle.get(3,1))*(r.get(4,1)-r.get(5,1));
Near32 36:54f86bc6fd80 41 T v = state.get(4,1);
Near32 36:54f86bc6fd80 42 T w = state.get(5,1);
Near32 36:54f86bc6fd80 43
Near32 36:54f86bc6fd80 44 r.set( r.get(1,1) + v*cos(r.get(3,1))*dt, 1,1);
Near32 36:54f86bc6fd80 45 r.set( r.get(2,1) + v*sin(r.get(3,1))*dt, 2,1);
Near32 36:54f86bc6fd80 46
Near32 36:54f86bc6fd80 47 T angle = (r.get(3,1) + dt*w);
Near32 36:54f86bc6fd80 48 if( angle < -PI)
Near32 36:54f86bc6fd80 49 {
Near32 36:54f86bc6fd80 50 angle = angle - PI*ceil(angle/PI);
Near32 36:54f86bc6fd80 51 }
Near32 36:54f86bc6fd80 52 else if( angle > PI)
Near32 36:54f86bc6fd80 53 {
Near32 36:54f86bc6fd80 54 angle = angle - PI*floor(angle/PI);
Near32 36:54f86bc6fd80 55 }
Near32 36:54f86bc6fd80 56
Near32 36:54f86bc6fd80 57 r.set( atan21(sin(angle), cos(angle)), 3,1);
Near32 36:54f86bc6fd80 58
Near32 36:54f86bc6fd80 59
Near32 36:54f86bc6fd80 60 /*----------------------------------------*/
Near32 36:54f86bc6fd80 61 /*Modele du moteur*/
Near32 36:54f86bc6fd80 62 /*----------------------------------------*/
Near32 36:54f86bc6fd80 63 //double r1 = bicycle.get(3,1)/bicycle.get(1,1)*(command.get(1,1)/bicycle.get(3,1)+command.get(2,1));
Near32 36:54f86bc6fd80 64 //double r2 = bicycle.get(3,1)/bicycle.get(1,1)*(command.get(1,1)/bicycle.get(3,1)-command.get(2,1));
Near32 36:54f86bc6fd80 65 T r1 = bicycle.get(1,1)/2*(command.get(1,1)+command.get(2,1));
Near32 36:54f86bc6fd80 66 T r2 = bicycle.get(1,1)/bicycle.get(3,1)*(command.get(1,1)-command.get(2,1));
Near32 36:54f86bc6fd80 67
Near32 36:54f86bc6fd80 68
Near32 36:54f86bc6fd80 69 r.set( r1, 4,1);
Near32 36:54f86bc6fd80 70 r.set( r2, 5,1);
Near32 36:54f86bc6fd80 71
Near32 36:54f86bc6fd80 72
Near32 36:54f86bc6fd80 73 /*----------------------------------------*/
Near32 36:54f86bc6fd80 74 /*----------------------------------------*/
Near32 36:54f86bc6fd80 75
Near32 36:54f86bc6fd80 76 return r;
Near32 36:54f86bc6fd80 77 }
Near32 36:54f86bc6fd80 78
Near32 36:54f86bc6fd80 79 template<typename T>
Near32 36:54f86bc6fd80 80 Mat<T> sensor_bicycle3( Mat<T> state, Mat<T> command, Mat<T> d_state, T dt)
Near32 36:54f86bc6fd80 81 {
Near32 36:54f86bc6fd80 82 /*
Near32 36:54f86bc6fd80 83 double angle = state.get(3,1);
Near32 36:54f86bc6fd80 84 if( angle < -PI)
Near32 36:54f86bc6fd80 85 {
Near32 36:54f86bc6fd80 86 angle = angle - PI*ceil(angle/PI);
Near32 36:54f86bc6fd80 87 }
Near32 36:54f86bc6fd80 88 else if( angle > PI)
Near32 36:54f86bc6fd80 89 {
Near32 36:54f86bc6fd80 90 angle = angle - PI*floor(angle/PI);
Near32 36:54f86bc6fd80 91 }
Near32 36:54f86bc6fd80 92
Near32 36:54f86bc6fd80 93 state.set( atan21(sin(angle), cos(angle)), 3,1);
Near32 36:54f86bc6fd80 94 */
Near32 36:54f86bc6fd80 95 return state;
Near32 36:54f86bc6fd80 96 }
Near32 36:54f86bc6fd80 97
Near32 36:54f86bc6fd80 98 template<typename T>
Near32 36:54f86bc6fd80 99 Mat<T> jmotion_bicycle3( Mat<T> state, Mat<T> command, T dt)
Near32 43:87bdce65341f 100 {
Near32 43:87bdce65341f 101 T h = pow(numeric_limits<T>::epsilon(),(T)0.5)*norme2(state)+ pow(numeric_limits<T>::epsilon(), (T)0.5);
Near32 36:54f86bc6fd80 102 Mat<T> var( (T)0, state.getLine(), state.getColumn());
Near32 36:54f86bc6fd80 103 var.set( h, 1,1);
Near32 36:54f86bc6fd80 104 Mat<T> G(motion_bicycle3(state+var, command, dt) - motion_bicycle3(state-var, command,dt));
Near32 36:54f86bc6fd80 105
Near32 36:54f86bc6fd80 106 for(int i=2;i<=state.getLine();i++)
Near32 36:54f86bc6fd80 107 {
Near32 36:54f86bc6fd80 108 var.set( (T)0, i-1,1);
Near32 36:54f86bc6fd80 109 var.set( h, i,1);
Near32 36:54f86bc6fd80 110 G = operatorL(G, motion_bicycle3(state+var, command, dt) - motion_bicycle3(state-var, command,dt) );
Near32 36:54f86bc6fd80 111 }
Near32 36:54f86bc6fd80 112
Near32 36:54f86bc6fd80 113
Near32 36:54f86bc6fd80 114 return ((T)1.0/(2*h))*G;
Near32 36:54f86bc6fd80 115 }
Near32 36:54f86bc6fd80 116
Near32 36:54f86bc6fd80 117 template<typename T>
Near32 36:54f86bc6fd80 118 Mat<T> jmotion_bicycle3_command(Mat<T> state, Mat<T> command, T dt)
Near32 36:54f86bc6fd80 119 {
Near32 43:87bdce65341f 120 T h = pow(numeric_limits<T>::epsilon(), (T)0.5)+sqrt(numeric_limits<T>::epsilon())*norme2(state);
Near32 36:54f86bc6fd80 121 Mat<T> var( (T)0, command.getLine(), command.getColumn());
Near32 36:54f86bc6fd80 122 var.set( h, 1,1);
Near32 36:54f86bc6fd80 123 Mat<T> G(motion_bicycle3(state, command+var, dt) - motion_bicycle3(state, command-var,dt));
Near32 36:54f86bc6fd80 124
Near32 36:54f86bc6fd80 125 for(int i=2;i<=command.getLine();i++)
Near32 36:54f86bc6fd80 126 {
Near32 36:54f86bc6fd80 127 var.set( (T)0, i-1,1);
Near32 36:54f86bc6fd80 128 var.set( h, i,1);
Near32 36:54f86bc6fd80 129 G = operatorL(G, motion_bicycle3(state, command+var, dt) - motion_bicycle3(state, command-var,dt) );
Near32 36:54f86bc6fd80 130 }
Near32 36:54f86bc6fd80 131
Near32 36:54f86bc6fd80 132
Near32 36:54f86bc6fd80 133 return (1.0/(2*h))*G;
Near32 36:54f86bc6fd80 134 }
Near32 36:54f86bc6fd80 135
Near32 36:54f86bc6fd80 136
Near32 36:54f86bc6fd80 137 template<typename T>
Near32 36:54f86bc6fd80 138 Mat<T> jsensor_bicycle3( Mat<T> state, Mat<T> command, Mat<T> d_state, T dt)
Near32 36:54f86bc6fd80 139 {
Near32 36:54f86bc6fd80 140 T h = pow(numeric_limits<T>::epsilon(), (T)0.5)+sqrt(numeric_limits<T>::epsilon())*norme2(state);
Near32 36:54f86bc6fd80 141 Mat<T> var((T)0, state.getLine(), state.getColumn());
Near32 36:54f86bc6fd80 142 var.set( h, 1,1);
Near32 36:54f86bc6fd80 143 Mat<T> H(sensor_bicycle3(state+var, command, d_state, dt) - sensor_bicycle3(state-var, command, d_state, dt));
Near32 36:54f86bc6fd80 144
Near32 36:54f86bc6fd80 145 for(int i=2;i<=state.getLine();i++)
Near32 36:54f86bc6fd80 146 {
Near32 36:54f86bc6fd80 147 var.set( (T)0, i-1,1);
Near32 36:54f86bc6fd80 148 var.set( h, i,1);
Near32 36:54f86bc6fd80 149 H = operatorL(H, sensor_bicycle3(state+var, command, d_state, dt) - sensor_bicycle3(state-var, command, d_state, dt) );
Near32 36:54f86bc6fd80 150 }
Near32 36:54f86bc6fd80 151
Near32 36:54f86bc6fd80 152
Near32 36:54f86bc6fd80 153 return ((T)1.0/(2*h))*H;
Near32 36:54f86bc6fd80 154 /*
Near32 36:54f86bc6fd80 155 double h = sqrt(numeric_limits<double>::epsilon())*10e2+sqrt(numeric_limits<double>::epsilon())*norme2(state);
Near32 36:54f86bc6fd80 156 Mat<double> var((double)0, state.getLine(), state.getColumn());
Near32 36:54f86bc6fd80 157 var.set( h, 1,1);
Near32 36:54f86bc6fd80 158 Mat<double> H(sensor_bicycle3(state+var, command, d_state, dt) - sensor_bicycle3(state-var, command, d_state, dt));
Near32 36:54f86bc6fd80 159
Near32 36:54f86bc6fd80 160 for(int i=2;i<=state.getLine();i++)
Near32 36:54f86bc6fd80 161 {
Near32 36:54f86bc6fd80 162 var.set( (double)0, i-1,1);
Near32 36:54f86bc6fd80 163 var.set( h, i,1);
Near32 36:54f86bc6fd80 164 H = operatorL(H, sensor_bicycle3(state+var, command, d_state, dt) - sensor_bicycle3(state-var, command, d_state, dt) );
Near32 36:54f86bc6fd80 165
Near32 36:54f86bc6fd80 166 }
Near32 36:54f86bc6fd80 167
Near32 36:54f86bc6fd80 168
Near32 36:54f86bc6fd80 169 return (1.0/(2*h))*H;
Near32 36:54f86bc6fd80 170 */
Near32 43:87bdce65341f 171 }
Near32 43:87bdce65341f 172 //int reduc = 16;
Near32 43:87bdce65341f 173
Near32 43:87bdce65341f 174 extern Serial logger;
Near32 43:87bdce65341f 175
Near32 43:87bdce65341f 176 /*---------------------------------------------------------------------------------------------------------*/
Near32 43:87bdce65341f 177 /*---------------------------------------------------------------------------------------------------------*/
Near32 43:87bdce65341f 178
Near32 43:87bdce65341f 179 template<typename T>
Near32 43:87bdce65341f 180 class Asserv
Near32 43:87bdce65341f 181 {
Near32 43:87bdce65341f 182 private :
Near32 43:87bdce65341f 183
Near32 43:87bdce65341f 184 Odometry* odometry;
Near32 43:87bdce65341f 185 EKF<T>* instanceEKF;
Near32 43:87bdce65341f 186 int nbrstate;
Near32 43:87bdce65341f 187 int nbrcontrol;
Near32 43:87bdce65341f 188 int nbrobs;
Near32 43:87bdce65341f 189 T dt;
Near32 43:87bdce65341f 190
Near32 43:87bdce65341f 191 T stdnoise;
Near32 43:87bdce65341f 192 Mat<T> X;
Near32 43:87bdce65341f 193 Mat<T> dX;
Near32 43:87bdce65341f 194 Mat<T> initX;
Near32 43:87bdce65341f 195 Mat<T> z;
Near32 43:87bdce65341f 196 Mat<T> u;
Near32 43:87bdce65341f 197 bool extended;
Near32 43:87bdce65341f 198 bool filterOn;
Near32 43:87bdce65341f 199
Near32 43:87bdce65341f 200 Mat<T> ki;
Near32 43:87bdce65341f 201 Mat<T> kp;
Near32 43:87bdce65341f 202 Mat<T> kd;
Near32 43:87bdce65341f 203
Near32 43:87bdce65341f 204 public :
Near32 43:87bdce65341f 205
Near32 43:87bdce65341f 206 T phi_r;
Near32 43:87bdce65341f 207 T phi_l;
Near32 43:87bdce65341f 208 T phi_max;
Near32 43:87bdce65341f 209 bool execution;
Near32 43:87bdce65341f 210 bool isarrived;
Near32 43:87bdce65341f 211
Near32 43:87bdce65341f 212 Asserv(Odometry* odometry)
Near32 43:87bdce65341f 213 {
Near32 43:87bdce65341f 214 /*Odometry*/
Near32 43:87bdce65341f 215 this->odometry = odometry;
Near32 44:d5f95af61243 216 phi_max = (T)100.0;
Near32 43:87bdce65341f 217
Near32 43:87bdce65341f 218 /*KalmanFilter*/
Near32 43:87bdce65341f 219 //double phi_max = 100;
Near32 43:87bdce65341f 220 /*en millimetres*/
Near32 43:87bdce65341f 221
Near32 43:87bdce65341f 222 nbrstate = 5;
Near32 43:87bdce65341f 223 nbrcontrol = 2;
Near32 43:87bdce65341f 224 nbrobs = 5;
Near32 43:87bdce65341f 225 dt = (T)0.05;
Near32 43:87bdce65341f 226 stdnoise = (T)0.05;
Near32 43:87bdce65341f 227
Near32 43:87bdce65341f 228 initX = Mat<T>((T)0, nbrstate, 1);
Near32 43:87bdce65341f 229 initX.set( (T)0, 3,1);
Near32 43:87bdce65341f 230 X = initX;
Near32 43:87bdce65341f 231
Near32 43:87bdce65341f 232 extended = true;
Near32 43:87bdce65341f 233 filterOn = true;
Near32 43:87bdce65341f 234
Near32 43:87bdce65341f 235 instanceEKF = new EKF<T>(nbrstate, nbrcontrol, nbrobs, dt, stdnoise, /*current state*/ initX, extended, filterOn);
Near32 43:87bdce65341f 236
Near32 43:87bdce65341f 237 instanceEKF->initMotion(motion_bicycle3);
Near32 43:87bdce65341f 238 instanceEKF->initSensor(sensor_bicycle3);
Near32 43:87bdce65341f 239 instanceEKF->initJMotion(jmotion_bicycle3);
Near32 43:87bdce65341f 240 //instanceEKF.initJMotionCommand(jmotion_bicycle3_command);
Near32 43:87bdce65341f 241 instanceEKF->initJSensor(jsensor_bicycle3);
Near32 43:87bdce65341f 242
Near32 43:87bdce65341f 243 /*desired State : (x y theta phiright phileft)*/
Near32 43:87bdce65341f 244 dX = Mat<T>((T)0, nbrstate, 1);
Near32 43:87bdce65341f 245 dX.set( (T)0, 1,1);
Near32 43:87bdce65341f 246 dX.set( (T)0, 2,1);
Near32 43:87bdce65341f 247 dX.set( (T)0, 3,1);
Near32 43:87bdce65341f 248 dX.set( (T)0, 4,1);
Near32 43:87bdce65341f 249 dX.set( (T)0, 5,1);
Near32 43:87bdce65341f 250
Near32 43:87bdce65341f 251 ki = Mat<T>((T)0, nbrcontrol, nbrstate);
Near32 43:87bdce65341f 252 kp = Mat<T>((T)0, nbrcontrol, nbrstate);
Near32 43:87bdce65341f 253 kd = Mat<T>((T)0, nbrcontrol, nbrstate);
Near32 43:87bdce65341f 254 //Mat<double> kdd((double)0.0015, nbrcontrol, nbrstate);
Near32 43:87bdce65341f 255
Near32 43:87bdce65341f 256 for(int i=1;i<=nbrstate;i++)
Near32 43:87bdce65341f 257 {
Near32 43:87bdce65341f 258 kp.set( (T)0.01, i, i);
Near32 43:87bdce65341f 259 kd.set( (T)0.0001, i, i);
Near32 43:87bdce65341f 260 ki.set( (T)0.0001, i, i);
Near32 43:87bdce65341f 261 }
Near32 43:87bdce65341f 262
Near32 43:87bdce65341f 263 instanceEKF->setKi(ki);
Near32 43:87bdce65341f 264 instanceEKF->setKp(kp);
Near32 43:87bdce65341f 265 instanceEKF->setKd(kd);
Near32 43:87bdce65341f 266 //instance.setKdd(kdd);
Near32 43:87bdce65341f 267
Near32 43:87bdce65341f 268 u = Mat<T>(transpose( instanceEKF->getCommand()) );
Near32 43:87bdce65341f 269
Near32 43:87bdce65341f 270 /*Observations*/
Near32 43:87bdce65341f 271 /*il nous faut 5 observation : mais on n'en met à jour que 3...*/
Near32 43:87bdce65341f 272 z = Mat<T>((T)0,5,1);
Near32 43:87bdce65341f 273 this->measurementCallback(&z);
Near32 43:87bdce65341f 274
Near32 43:87bdce65341f 275 /*----------------------------------------------------------------------------------------------*/
Near32 43:87bdce65341f 276 isarrived = true;
Near32 43:87bdce65341f 277 execution = false;
Near32 43:87bdce65341f 278
Near32 43:87bdce65341f 279 }
Near32 43:87bdce65341f 280
Near32 43:87bdce65341f 281 ~Asserv()
Near32 43:87bdce65341f 282 {
Near32 43:87bdce65341f 283 delete instanceEKF;
Near32 43:87bdce65341f 284 }
Near32 43:87bdce65341f 285
Near32 43:87bdce65341f 286 void setGoal(T x, T y, T theta)
Near32 43:87bdce65341f 287 {
Near32 43:87bdce65341f 288 dX.set(x,1,1);
Near32 43:87bdce65341f 289 dX.set(y,2,1);
Near32 43:87bdce65341f 290 dX.set(theta,3,1);
Near32 43:87bdce65341f 291 dX.set((T)0,4,1);
Near32 43:87bdce65341f 292 dX.set((T)0,5,1);
Near32 43:87bdce65341f 293 execution = true;
Near32 43:87bdce65341f 294 isarrived = false;
Near32 43:87bdce65341f 295
Near32 43:87bdce65341f 296 }
Near32 43:87bdce65341f 297
Near32 43:87bdce65341f 298 bool isArrived() { return isarrived;}
Near32 43:87bdce65341f 299
Near32 43:87bdce65341f 300 void stop()
Near32 43:87bdce65341f 301 {
Near32 43:87bdce65341f 302 execution = false;
Near32 43:87bdce65341f 303 }
Near32 43:87bdce65341f 304
Near32 43:87bdce65341f 305 void update(T deltat)
Near32 43:87bdce65341f 306 {
Near32 43:87bdce65341f 307 if(execution)
Near32 43:87bdce65341f 308 {
Near32 43:87bdce65341f 309 dt = deltat;
Near32 43:87bdce65341f 310 /*Asservissement*/
Near32 43:87bdce65341f 311 this->measurementCallback(&z);
Near32 43:87bdce65341f 312 instanceEKF->measurement_Callback( z, dX, true );
Near32 43:87bdce65341f 313 instanceEKF->state_Callback();
Near32 43:87bdce65341f 314 //instance.setX(z);
Near32 43:87bdce65341f 315 X = instanceEKF->getX();
Near32 43:87bdce65341f 316
Near32 43:87bdce65341f 317 //instance.computeCommand(dX, (double)dt, -2);
Near32 43:87bdce65341f 318 instanceEKF->computeCommand(dX, (T)dt, -2);
Near32 44:d5f95af61243 319 phi_l = instanceEKF->getCommand().get(1,1);
Near32 44:d5f95af61243 320 phi_r = instanceEKF->getCommand().get(2,1);
Near32 43:87bdce65341f 321 }
Near32 43:87bdce65341f 322 else
Near32 43:87bdce65341f 323 {
Near32 43:87bdce65341f 324 phi_r = (T)0;
Near32 43:87bdce65341f 325 phi_l = (T)0;
Near32 43:87bdce65341f 326 }
Near32 43:87bdce65341f 327 }
Near32 43:87bdce65341f 328
Near32 43:87bdce65341f 329 void measurementCallback( Mat<T>* z)
Near32 43:87bdce65341f 330 {
Near32 43:87bdce65341f 331 z->set( (T)/*conversionUnitée mm */this->odometry->getX(), 1,1);
Near32 43:87bdce65341f 332 z->set( (T)/*conversionUnitée mm*/this->odometry->getY(), 2,1);
Near32 43:87bdce65341f 333 T theta = (T)this->odometry->getTheta();
Near32 43:87bdce65341f 334 theta = atan21(sin(theta),cos(theta));
Near32 43:87bdce65341f 335 z->set( (double)/*conversionUnitée rad*/theta, 3,1);//odometry->getTheta(), 3,1);
Near32 43:87bdce65341f 336 T vx = (T)this->odometry->getVx();
Near32 43:87bdce65341f 337 T vy = (T)this->odometry->getVy();
Near32 43:87bdce65341f 338 z->set( sqrt(vx*vx+vy*vy),4,1);
Near32 43:87bdce65341f 339 z->set( (T)odometry->getW(),5,1);
Near32 43:87bdce65341f 340
Near32 44:d5f95af61243 341 //transpose(*z).afficherMblue();
Near32 43:87bdce65341f 342 }
Near32 43:87bdce65341f 343
Near32 43:87bdce65341f 344 Mat<T> getX()
Near32 43:87bdce65341f 345 {
Near32 43:87bdce65341f 346 return X;
Near32 43:87bdce65341f 347 }
Near32 43:87bdce65341f 348
Near32 43:87bdce65341f 349 T getPhiR()
Near32 43:87bdce65341f 350 {
Near32 43:87bdce65341f 351 return phi_r;
Near32 43:87bdce65341f 352 }
Near32 43:87bdce65341f 353
Near32 43:87bdce65341f 354 T getPhiL()
Near32 43:87bdce65341f 355 {
Near32 43:87bdce65341f 356 return phi_l;
Near32 43:87bdce65341f 357 }
Near32 43:87bdce65341f 358
Near32 43:87bdce65341f 359 T getPhiMax()
Near32 43:87bdce65341f 360 {
Near32 43:87bdce65341f 361 return phi_max;
Near32 43:87bdce65341f 362 }
Near32 43:87bdce65341f 363
Near32 43:87bdce65341f 364
Near32 43:87bdce65341f 365 };