ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

Asservissement/Asserv.h@42:fcb48e2fc426, 2015-03-18 (annotated)

Committer:
Near32
Date:
Wed Mar 18 13:47:40 2015 +0000
Revision:
42:fcb48e2fc426
Parent:
41:c04c2ec37aad
Child:
43:87bdce65341f
liaison serie usb car le bluetooth est visiblement tr?s capricieux...; test de compilation et de debuggage par gdb en cours...

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