Yo Fleyt / Mbed 2 deprecated roboticLab_withclass_3_July

test morning

Dependencies:   ISR_Mini-explorer mbed

Fork of roboticLab_withclass_3_July by Georgios Tsamis

MiniExplorerCoimbra.cpp@14:696187e74411, 2017-07-12 (annotated)

Committer:
Ludwigfr
Date:
Wed Jul 12 18:08:07 2017 +0000
Revision:
14:696187e74411
Parent:
13:b0ddd71e8f08 
lol

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Ludwigfr 0:9f7ee7ed13e4 1 #include "MiniExplorerCoimbra.hpp"
Ludwigfr 0:9f7ee7ed13e4 2 #include "robot.h"
Ludwigfr 0:9f7ee7ed13e4 3
Ludwigfr 0:9f7ee7ed13e4 4 #define PI 3.14159
Ludwigfr 0:9f7ee7ed13e4 5
Ludwigfr 9:1cc27f33d3e1 6 MiniExplorerCoimbra::MiniExplorerCoimbra(float defaultXWorld, float defaultYWorld, float defaultThetaWorld, float widthRealMap, float heightRealMap):map(widthRealMap,heightRealMap,12,8),sonarLeft(10*PI/36,-4,4),sonarFront(0,0,5),sonarRight(-10*PI/36,4,4),covariancePositionEstimationK(3,3){
Ludwigfr 0:9f7ee7ed13e4 7 i2c1.frequency(100000);
Ludwigfr 0:9f7ee7ed13e4 8 initRobot(); //Initializing the robot
Ludwigfr 0:9f7ee7ed13e4 9 pc.baud(9600); // baud for the pc communication
Ludwigfr 0:9f7ee7ed13e4 10
Ludwigfr 0:9f7ee7ed13e4 11 measure_always_on();//TODO check if needed
Ludwigfr 0:9f7ee7ed13e4 12
Ludwigfr 0:9f7ee7ed13e4 13 this->setXYThetaAndXYThetaWorld(defaultXWorld,defaultYWorld,defaultThetaWorld);
Ludwigfr 0:9f7ee7ed13e4 14 this->radiusWheels=3.25;
Ludwigfr 14:696187e74411 15 this->distanceWheels=7.18;
Ludwigfr 0:9f7ee7ed13e4 16
Ludwigfr 8:072a76960e27 17 //go to point
geotsam 1:20f48907c726 18 this->k_linear=10;
geotsam 1:20f48907c726 19 this->k_angular=200;
Ludwigfr 8:072a76960e27 20
Ludwigfr 8:072a76960e27 21 //go to point with angle
Ludwigfr 8:072a76960e27 22 this->khro=0.3;
Ludwigfr 8:072a76960e27 23 this->ka=0.8;
Ludwigfr 8:072a76960e27 24 this->kb=-0.36;
Ludwigfr 8:072a76960e27 25 this->kv=200;//velocity
Ludwigfr 8:072a76960e27 26
Ludwigfr 8:072a76960e27 27 //go to line kh > 0, kd > 0 (200,5), dont turn to line enough, (10,10) turn on itself,
Ludwigfr 8:072a76960e27 28 this->kh=650;
Ludwigfr 8:072a76960e27 29 this->kd=10;//previous 5
Ludwigfr 8:072a76960e27 30
geotsam 1:20f48907c726 31 this->speed=300;
Ludwigfr 0:9f7ee7ed13e4 32
Ludwigfr 14:696187e74411 33 this->rangeForce=75;
Ludwigfr 14:696187e74411 34 this->attractionConstantForce=4;
Ludwigfr 9:1cc27f33d3e1 35 //-90 ok with attraction impacted by distance
Ludwigfr 14:696187e74411 36 this->repulsionConstantForce=-200;
Ludwigfr 5:19f24c363418 37
Ludwigfr 14:696187e74411 38 this->D_cm=0;
Ludwigfr 14:696187e74411 39 this->L_cm=0;
Ludwigfr 14:696187e74411 40
Ludwigfr 14:696187e74411 41 this->ticks2d_Special=0;
Ludwigfr 14:696187e74411 42 this->ticks2e_Special=0;
Ludwigfr 0:9f7ee7ed13e4 43 }
Ludwigfr 0:9f7ee7ed13e4 44
Ludwigfr 0:9f7ee7ed13e4 45 void MiniExplorerCoimbra::setXYThetaAndXYThetaWorld(float defaultXWorld, float defaultYWorld, float defaultThetaWorld){
Ludwigfr 0:9f7ee7ed13e4 46 this->xWorld=defaultXWorld;
Ludwigfr 0:9f7ee7ed13e4 47 this->yWorld=defaultYWorld;
Ludwigfr 0:9f7ee7ed13e4 48 this->thetaWorld=defaultThetaWorld;
Ludwigfr 0:9f7ee7ed13e4 49 X=defaultYWorld;
Ludwigfr 0:9f7ee7ed13e4 50 Y=-defaultXWorld;
Ludwigfr 0:9f7ee7ed13e4 51 if(defaultThetaWorld < -PI/2)
Ludwigfr 0:9f7ee7ed13e4 52 theta=PI/2+PI-defaultThetaWorld;
Ludwigfr 0:9f7ee7ed13e4 53 else
Ludwigfr 0:9f7ee7ed13e4 54 theta=defaultThetaWorld-PI/2;
Ludwigfr 14:696187e74411 55 this->myX_r=X;
Ludwigfr 14:696187e74411 56 this->myY_r=Y;
Ludwigfr 14:696187e74411 57 this->mytheta_r=theta;
Ludwigfr 14:696187e74411 58
Ludwigfr 14:696187e74411 59 this->myX_r_correct=X;
Ludwigfr 14:696187e74411 60 this->myY_r_correct=Y;
Ludwigfr 14:696187e74411 61 this->mytheta_r_correct=theta;
Ludwigfr 0:9f7ee7ed13e4 62 }
Ludwigfr 0:9f7ee7ed13e4 63
Ludwigfr 0:9f7ee7ed13e4 64 void MiniExplorerCoimbra::myOdometria(){
Ludwigfr 0:9f7ee7ed13e4 65 Odometria();
Ludwigfr 0:9f7ee7ed13e4 66 this->xWorld=-Y;
Ludwigfr 0:9f7ee7ed13e4 67 this->yWorld=X;
Ludwigfr 0:9f7ee7ed13e4 68 if(theta >PI/2)
Ludwigfr 0:9f7ee7ed13e4 69 this->thetaWorld=-PI+(theta-PI/2);
Ludwigfr 0:9f7ee7ed13e4 70 else
Ludwigfr 0:9f7ee7ed13e4 71 this->thetaWorld=theta+PI/2;
Ludwigfr 0:9f7ee7ed13e4 72 }
Ludwigfr 0:9f7ee7ed13e4 73
geotsam 1:20f48907c726 74 void MiniExplorerCoimbra::go_to_point(float targetXWorld, float targetYWorld) {
geotsam 1:20f48907c726 75
geotsam 1:20f48907c726 76 float angleError; //angle error
geotsam 1:20f48907c726 77 float d; //distance from target
geotsam 1:20f48907c726 78 float angularLeft, angularRight, linear, angular;
geotsam 1:20f48907c726 79 int speed=300;
geotsam 1:20f48907c726 80
geotsam 1:20f48907c726 81 do {
geotsam 1:20f48907c726 82 //Updating X,Y and theta with the odometry values
geotsam 1:20f48907c726 83 this->myOdometria();
geotsam 1:20f48907c726 84
geotsam 1:20f48907c726 85 //Computing angle error and distance towards the target value
geotsam 1:20f48907c726 86 angleError = atan2((targetYWorld-this->yWorld),(targetXWorld-this->xWorld))-this->thetaWorld;
geotsam 1:20f48907c726 87 if(angleError>PI) angleError=-(angleError-PI);
geotsam 1:20f48907c726 88 else if(angleError<-PI) angleError=-(angleError+PI);
geotsam 1:20f48907c726 89 pc.printf("\n\r error=%f",angleError);
geotsam 1:20f48907c726 90
geotsam 1:20f48907c726 91 d=this->dist(this->xWorld, this->yWorld, targetXWorld, targetYWorld);
geotsam 1:20f48907c726 92 pc.printf("\n\r dist=%f/n", d);
geotsam 1:20f48907c726 93
geotsam 1:20f48907c726 94 //Computing linear and angular velocities
geotsam 1:20f48907c726 95 linear=k_linear*d;
geotsam 1:20f48907c726 96 angular=k_angular*angleError;
geotsam 1:20f48907c726 97 angularLeft=(linear-0.5*this->distanceWheels*angular)/this->radiusWheels;
geotsam 1:20f48907c726 98 angularRight=(linear+0.5*this->distanceWheels*angular)/this->radiusWheels;
geotsam 1:20f48907c726 99
geotsam 1:20f48907c726 100
geotsam 1:20f48907c726 101 //Normalize speed for motors
geotsam 1:20f48907c726 102 if(angularLeft>angularRight) {
geotsam 1:20f48907c726 103 angularRight=speed*angularRight/angularLeft;
geotsam 1:20f48907c726 104 angularLeft=speed;
geotsam 1:20f48907c726 105 } else {
geotsam 1:20f48907c726 106 angularLeft=speed*angularLeft/angularRight;
geotsam 1:20f48907c726 107 angularRight=speed;
geotsam 1:20f48907c726 108 }
geotsam 1:20f48907c726 109
geotsam 1:20f48907c726 110 pc.printf("\n\r X=%f", this->xWorld);
geotsam 1:20f48907c726 111 pc.printf("\n\r Y=%f", this->yWorld);
geotsam 1:20f48907c726 112 pc.printf("\n\r theta=%f", this->thetaWorld);
geotsam 1:20f48907c726 113
geotsam 1:20f48907c726 114 //Updating motor velocities
geotsam 1:20f48907c726 115 if(angularLeft>0){
geotsam 1:20f48907c726 116 leftMotor(1,angularLeft);
geotsam 1:20f48907c726 117 }
geotsam 1:20f48907c726 118 else{
geotsam 1:20f48907c726 119 leftMotor(0,-angularLeft);
geotsam 1:20f48907c726 120 }
geotsam 1:20f48907c726 121
geotsam 1:20f48907c726 122 if(angularRight>0){
geotsam 1:20f48907c726 123 rightMotor(1,angularRight);
geotsam 1:20f48907c726 124 }
geotsam 1:20f48907c726 125 else{
geotsam 1:20f48907c726 126 rightMotor(0,-angularRight);
geotsam 1:20f48907c726 127 }
geotsam 1:20f48907c726 128
Ludwigfr 5:19f24c363418 129 //wait(0.5);
geotsam 1:20f48907c726 130 } while(d>1);
geotsam 1:20f48907c726 131
geotsam 1:20f48907c726 132 //Stop at the end
geotsam 1:20f48907c726 133 leftMotor(1,0);
geotsam 1:20f48907c726 134 rightMotor(1,0);
geotsam 1:20f48907c726 135 }
geotsam 1:20f48907c726 136
Ludwigfr 2:11cd5173aa36 137 void MiniExplorerCoimbra::test_procedure_lab2(int nbIteration){
Ludwigfr 2:11cd5173aa36 138 for(int i=0;i<nbIteration;i++){
Ludwigfr 2:11cd5173aa36 139 this->randomize_and_map();
Ludwigfr 5:19f24c363418 140 //this->print_map_with_robot_position();
Ludwigfr 2:11cd5173aa36 141 }
Ludwigfr 3:37345c109dfc 142 while(1)
Ludwigfr 3:37345c109dfc 143 this->print_map_with_robot_position();
Ludwigfr 2:11cd5173aa36 144 }
Ludwigfr 2:11cd5173aa36 145
Ludwigfr 0:9f7ee7ed13e4 146 //generate a position randomly and makes the robot go there while updating the map
Ludwigfr 0:9f7ee7ed13e4 147 void MiniExplorerCoimbra::randomize_and_map() {
Ludwigfr 0:9f7ee7ed13e4 148 //TODO check that it's aurelien's work
Ludwigfr 2:11cd5173aa36 149 float movementOnX=rand()%(int)(this->map.widthRealMap);
Ludwigfr 2:11cd5173aa36 150 float movementOnY=rand()%(int)(this->map.heightRealMap);
Ludwigfr 3:37345c109dfc 151
Ludwigfr 3:37345c109dfc 152 float targetXWorld = movementOnX;
Ludwigfr 3:37345c109dfc 153 float targetYWorld = movementOnY;
Ludwigfr 3:37345c109dfc 154 float targetAngleWorld = 2*((float)(rand()%31416)-15708)/10000.0;
Ludwigfr 0:9f7ee7ed13e4 155
Ludwigfr 3:37345c109dfc 156 //target between (0,0) and (widthRealMap,heightRealMap)
Ludwigfr 0:9f7ee7ed13e4 157 this->go_to_point_with_angle(targetXWorld, targetYWorld, targetAngleWorld);
Ludwigfr 0:9f7ee7ed13e4 158 }
Ludwigfr 0:9f7ee7ed13e4 159
Ludwigfr 2:11cd5173aa36 160 void MiniExplorerCoimbra::test_sonars_and_map(int nbIteration){
Ludwigfr 2:11cd5173aa36 161 float leftMm;
Ludwigfr 2:11cd5173aa36 162 float frontMm;
Ludwigfr 2:11cd5173aa36 163 float rightMm;
Ludwigfr 2:11cd5173aa36 164 this->myOdometria();
Ludwigfr 2:11cd5173aa36 165 this->print_map_with_robot_position();
Ludwigfr 2:11cd5173aa36 166 for(int i=0;i<nbIteration;i++){
Ludwigfr 2:11cd5173aa36 167 leftMm = get_distance_left_sensor();
Ludwigfr 2:11cd5173aa36 168 frontMm = get_distance_front_sensor();
Ludwigfr 2:11cd5173aa36 169 rightMm = get_distance_right_sensor();
Ludwigfr 3:37345c109dfc 170 pc.printf("\n\r 1 leftMm= %f",leftMm);
Ludwigfr 3:37345c109dfc 171 pc.printf("\n\r 1 frontMm= %f",frontMm);
Ludwigfr 3:37345c109dfc 172 pc.printf("\n\r 1 rightMm= %f",rightMm);
Ludwigfr 2:11cd5173aa36 173 this->update_sonar_values(leftMm, frontMm, rightMm);
Ludwigfr 2:11cd5173aa36 174 this->print_map_with_robot_position();
Ludwigfr 3:37345c109dfc 175 wait(1);
Ludwigfr 2:11cd5173aa36 176 }
Ludwigfr 2:11cd5173aa36 177 }
Ludwigfr 2:11cd5173aa36 178
Ludwigfr 2:11cd5173aa36 179
Ludwigfr 2:11cd5173aa36 180 //generate a position randomly and makes the robot go there while updating the map
Ludwigfr 0:9f7ee7ed13e4 181 //move of targetXWorld and targetYWorld ending in a targetAngleWorld
Ludwigfr 0:9f7ee7ed13e4 182 void MiniExplorerCoimbra::go_to_point_with_angle(float targetXWorld, float targetYWorld, float targetAngleWorld) {
Ludwigfr 0:9f7ee7ed13e4 183 bool keepGoing=true;
Ludwigfr 0:9f7ee7ed13e4 184 float leftMm;
Ludwigfr 0:9f7ee7ed13e4 185 float frontMm;
Ludwigfr 0:9f7ee7ed13e4 186 float rightMm;
Ludwigfr 0:9f7ee7ed13e4 187 float dt;
Ludwigfr 0:9f7ee7ed13e4 188 Timer t;
Ludwigfr 0:9f7ee7ed13e4 189 float distanceToTarget;
Ludwigfr 0:9f7ee7ed13e4 190 do {
Ludwigfr 0:9f7ee7ed13e4 191 //Timer stuff
Ludwigfr 0:9f7ee7ed13e4 192 dt = t.read();
Ludwigfr 0:9f7ee7ed13e4 193 t.reset();
Ludwigfr 0:9f7ee7ed13e4 194 t.start();
Ludwigfr 0:9f7ee7ed13e4 195
Ludwigfr 0:9f7ee7ed13e4 196 //Updating X,Y and theta with the odometry values
Ludwigfr 0:9f7ee7ed13e4 197 this->myOdometria();
Ludwigfr 2:11cd5173aa36 198 leftMm = get_distance_left_sensor();
Ludwigfr 2:11cd5173aa36 199 frontMm = get_distance_front_sensor();
Ludwigfr 2:11cd5173aa36 200 rightMm = get_distance_right_sensor();
Ludwigfr 2:11cd5173aa36 201 //if in dangerzone 150 mm
Ludwigfr 0:9f7ee7ed13e4 202 if((frontMm < 150 && frontMm > 0)|| (leftMm <150 && leftMm > 0) || (rightMm <150 && rightMm > 0) ){
Ludwigfr 2:11cd5173aa36 203 //stop motors
Ludwigfr 0:9f7ee7ed13e4 204 leftMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 205 rightMotor(1,0);
Ludwigfr 2:11cd5173aa36 206 //update the map
Ludwigfr 0:9f7ee7ed13e4 207 this->update_sonar_values(leftMm, frontMm, rightMm);
Ludwigfr 0:9f7ee7ed13e4 208 this->myOdometria();
Ludwigfr 0:9f7ee7ed13e4 209 keepGoing=false;
Ludwigfr 2:11cd5173aa36 210 this->do_half_flip();
Ludwigfr 0:9f7ee7ed13e4 211 }else{
Ludwigfr 0:9f7ee7ed13e4 212 //if not in danger zone continue as usual
Ludwigfr 0:9f7ee7ed13e4 213 this->update_sonar_values(leftMm, frontMm, rightMm);
Ludwigfr 2:11cd5173aa36 214 //Updating motor velocities
Ludwigfr 0:9f7ee7ed13e4 215 distanceToTarget=this->update_angular_speed_wheels_go_to_point_with_angle(targetXWorld,targetYWorld,targetAngleWorld,dt);
Ludwigfr 5:19f24c363418 216 //wait(0.2);
Ludwigfr 0:9f7ee7ed13e4 217 //Timer stuff
Ludwigfr 0:9f7ee7ed13e4 218 t.stop();
Ludwigfr 2:11cd5173aa36 219 pc.printf("\n\rdist to target= %f",distanceToTarget);
Ludwigfr 0:9f7ee7ed13e4 220 }
Ludwigfr 3:37345c109dfc 221 } while((distanceToTarget>2 || (abs(targetAngleWorld-this->thetaWorld)>PI/3)) && keepGoing);
Ludwigfr 0:9f7ee7ed13e4 222
Ludwigfr 0:9f7ee7ed13e4 223 //Stop at the end
Ludwigfr 0:9f7ee7ed13e4 224 leftMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 225 rightMotor(1,0);
Ludwigfr 2:11cd5173aa36 226 pc.printf("\r\nReached Target!");
Ludwigfr 0:9f7ee7ed13e4 227 }
Ludwigfr 0:9f7ee7ed13e4 228
geotsam 1:20f48907c726 229 //move of targetXWorld and targetYWorld ending in a targetAngleWorld
geotsam 1:20f48907c726 230 void MiniExplorerCoimbra::go_to_point_with_angle_first_lab(float targetXWorld, float targetYWorld, float targetAngleWorld) {
geotsam 1:20f48907c726 231 float dt;
geotsam 1:20f48907c726 232 Timer t;
geotsam 1:20f48907c726 233 float distanceToTarget;
geotsam 1:20f48907c726 234 do {
geotsam 1:20f48907c726 235 //Timer stuff
geotsam 1:20f48907c726 236 dt = t.read();
geotsam 1:20f48907c726 237 t.reset();
geotsam 1:20f48907c726 238 t.start();
geotsam 1:20f48907c726 239
geotsam 1:20f48907c726 240 //Updating X,Y and theta with the odometry values
geotsam 1:20f48907c726 241 this->myOdometria();
geotsam 1:20f48907c726 242
geotsam 1:20f48907c726 243 //Updating motor velocities
geotsam 1:20f48907c726 244 distanceToTarget=this->update_angular_speed_wheels_go_to_point_with_angle(targetXWorld,targetYWorld,targetAngleWorld,dt);
geotsam 1:20f48907c726 245
Ludwigfr 5:19f24c363418 246 //wait(0.2);
geotsam 1:20f48907c726 247 //Timer stuff
geotsam 1:20f48907c726 248 t.stop();
geotsam 1:20f48907c726 249 pc.printf("\n\rdist to target= %f",distanceToTarget);
geotsam 1:20f48907c726 250
geotsam 1:20f48907c726 251 } while(distanceToTarget>1 || (abs(targetAngleWorld-this->thetaWorld)>0.1));
geotsam 1:20f48907c726 252
geotsam 1:20f48907c726 253 //Stop at the end
geotsam 1:20f48907c726 254 leftMotor(1,0);
geotsam 1:20f48907c726 255 rightMotor(1,0);
geotsam 1:20f48907c726 256 pc.printf("\r\nReached Target!");
geotsam 1:20f48907c726 257 }
geotsam 1:20f48907c726 258
Ludwigfr 0:9f7ee7ed13e4 259 float MiniExplorerCoimbra::update_angular_speed_wheels_go_to_point_with_angle(float targetXWorld, float targetYWorld, float targetAngleWorld, float dt){
Ludwigfr 0:9f7ee7ed13e4 260 //compute_angles_and_distance
Ludwigfr 0:9f7ee7ed13e4 261 //atan2 take the deplacement on x and the deplacement on y as parameters
Ludwigfr 0:9f7ee7ed13e4 262 float angleToPoint = atan2((targetYWorld-this->yWorld),(targetXWorld-this->xWorld))-this->thetaWorld;
geotsam 1:20f48907c726 263
geotsam 1:20f48907c726 264 if(angleToPoint>PI) angleToPoint=-(angleToPoint-PI);
Ludwigfr 3:37345c109dfc 265 else if(angleToPoint<-PI) angleToPoint=-(angleToPoint+PI);
geotsam 1:20f48907c726 266
Ludwigfr 3:37345c109dfc 267 //rho is the distance to the point of arrival
Ludwigfr 0:9f7ee7ed13e4 268 float rho = dist(targetXWorld,targetYWorld,this->xWorld,this->yWorld);
Ludwigfr 0:9f7ee7ed13e4 269 float distanceToTarget = rho;
Ludwigfr 0:9f7ee7ed13e4 270 //TODO check that
Ludwigfr 0:9f7ee7ed13e4 271 float beta = targetAngleWorld-angleToPoint-this->thetaWorld;
Ludwigfr 0:9f7ee7ed13e4 272
Ludwigfr 0:9f7ee7ed13e4 273 //Computing angle error and distance towards the target value
Ludwigfr 0:9f7ee7ed13e4 274 rho += dt*(-this->khro*cos(angleToPoint)*rho);
Ludwigfr 0:9f7ee7ed13e4 275 float temp = angleToPoint;
Ludwigfr 0:9f7ee7ed13e4 276 angleToPoint += dt*(this->khro*sin(angleToPoint)-this->ka*angleToPoint-this->kb*beta);
Ludwigfr 0:9f7ee7ed13e4 277 beta += dt*(-this->khro*sin(temp));
Ludwigfr 0:9f7ee7ed13e4 278
Ludwigfr 0:9f7ee7ed13e4 279 //Computing linear and angular velocities
Ludwigfr 0:9f7ee7ed13e4 280 float linear;
Ludwigfr 0:9f7ee7ed13e4 281 float angular;
Ludwigfr 0:9f7ee7ed13e4 282 if(angleToPoint>=-1.5708 && angleToPoint<=1.5708){
Ludwigfr 0:9f7ee7ed13e4 283 linear=this->khro*rho;
Ludwigfr 0:9f7ee7ed13e4 284 angular=this->ka*angleToPoint+this->kb*beta;
Ludwigfr 0:9f7ee7ed13e4 285 }
Ludwigfr 0:9f7ee7ed13e4 286 else{
Ludwigfr 0:9f7ee7ed13e4 287 linear=-this->khro*rho;
Ludwigfr 0:9f7ee7ed13e4 288 angular=-this->ka*angleToPoint-this->kb*beta;
Ludwigfr 0:9f7ee7ed13e4 289 }
geotsam 1:20f48907c726 290
geotsam 1:20f48907c726 291 float angularLeft=(linear-0.5*this->distanceWheels*angular)/this->radiusWheels;
geotsam 1:20f48907c726 292 float angularRight=(linear+0.5*this->distanceWheels*angular)/this->radiusWheels;
geotsam 1:20f48907c726 293
geotsam 1:20f48907c726 294 //Slowing down at the end for more precision
geotsam 1:20f48907c726 295 if (distanceToTarget<30) {
geotsam 1:20f48907c726 296 this->speed = distanceToTarget*10;
geotsam 1:20f48907c726 297 }
Ludwigfr 0:9f7ee7ed13e4 298
Ludwigfr 0:9f7ee7ed13e4 299 //Normalize speed for motors
geotsam 1:20f48907c726 300 if(angularLeft>angularRight) {
geotsam 1:20f48907c726 301 angularRight=this->speed*angularRight/angularLeft;
geotsam 1:20f48907c726 302 angularLeft=this->speed;
Ludwigfr 0:9f7ee7ed13e4 303 } else {
geotsam 1:20f48907c726 304 angularLeft=this->speed*angularLeft/angularRight;
geotsam 1:20f48907c726 305 angularRight=this->speed;
Ludwigfr 0:9f7ee7ed13e4 306 }
Ludwigfr 0:9f7ee7ed13e4 307
Ludwigfr 0:9f7ee7ed13e4 308 //compute_linear_angular_velocities
geotsam 1:20f48907c726 309 leftMotor(1,angularLeft);
geotsam 1:20f48907c726 310 rightMotor(1,angularRight);
Ludwigfr 0:9f7ee7ed13e4 311
Ludwigfr 0:9f7ee7ed13e4 312 return distanceToTarget;
Ludwigfr 0:9f7ee7ed13e4 313 }
Ludwigfr 0:9f7ee7ed13e4 314
Ludwigfr 0:9f7ee7ed13e4 315 void MiniExplorerCoimbra::update_sonar_values(float leftMm,float frontMm,float rightMm){
Ludwigfr 0:9f7ee7ed13e4 316 float xWorldCell;
Ludwigfr 0:9f7ee7ed13e4 317 float yWorldCell;
Ludwigfr 3:37345c109dfc 318 float probaLeft;
Ludwigfr 3:37345c109dfc 319 float probaFront;
Ludwigfr 3:37345c109dfc 320 float probaRight;
Ludwigfr 3:37345c109dfc 321 float leftCm=leftMm/10;
Ludwigfr 3:37345c109dfc 322 float frontCm=frontMm/10;
Ludwigfr 3:37345c109dfc 323 float rightCm=rightMm/10;
Ludwigfr 0:9f7ee7ed13e4 324 for(int i=0;i<this->map.nbCellWidth;i++){
Ludwigfr 0:9f7ee7ed13e4 325 for(int j=0;j<this->map.nbCellHeight;j++){
Ludwigfr 0:9f7ee7ed13e4 326 xWorldCell=this->map.cell_width_coordinate_to_world(i);
Ludwigfr 0:9f7ee7ed13e4 327 yWorldCell=this->map.cell_height_coordinate_to_world(j);
Ludwigfr 3:37345c109dfc 328
Ludwigfr 3:37345c109dfc 329 probaLeft=this->sonarLeft.compute_probability_t(leftCm,xWorldCell,yWorldCell,this->xWorld,this->yWorld,this->thetaWorld);
Ludwigfr 3:37345c109dfc 330 probaFront=this->sonarFront.compute_probability_t(frontCm,xWorldCell,yWorldCell,this->xWorld,this->yWorld,this->thetaWorld);
Ludwigfr 3:37345c109dfc 331 probaRight=this->sonarRight.compute_probability_t(rightCm,xWorldCell,yWorldCell,this->xWorld,this->yWorld,this->thetaWorld);
Ludwigfr 3:37345c109dfc 332
Ludwigfr 3:37345c109dfc 333 /*
Ludwigfr 3:37345c109dfc 334 pc.printf("\n\r leftCm= %f",leftCm);
Ludwigfr 3:37345c109dfc 335 pc.printf("\n\r frontCm= %f",frontCm);
Ludwigfr 3:37345c109dfc 336 pc.printf("\n\r rightCm= %f",rightCm);
Ludwigfr 3:37345c109dfc 337 */
Ludwigfr 3:37345c109dfc 338 /*
Ludwigfr 3:37345c109dfc 339 pc.printf("\n\r probaLeft= %f",probaLeft);
Ludwigfr 3:37345c109dfc 340 pc.printf("\n\r probaFront= %f",probaFront);
Ludwigfr 3:37345c109dfc 341 pc.printf("\n\r probaRight= %f",probaRight);
Ludwigfr 3:37345c109dfc 342 if(probaLeft> 1 || probaLeft < 0 || probaFront> 1 || probaFront < 0 ||probaRight> 1 || probaRight < 0)){
Ludwigfr 3:37345c109dfc 343 pwm_buzzer.pulsewidth_us(250);
Ludwigfr 3:37345c109dfc 344 wait_ms(50);
Ludwigfr 3:37345c109dfc 345 pwm_buzzer.pulsewidth_us(0);
Ludwigfr 3:37345c109dfc 346 wait(20);
Ludwigfr 3:37345c109dfc 347 pwm_buzzer.pulsewidth_us(250);
Ludwigfr 3:37345c109dfc 348 wait_ms(50);
Ludwigfr 3:37345c109dfc 349 pwm_buzzer.pulsewidth_us(0);
Ludwigfr 3:37345c109dfc 350 }
Ludwigfr 3:37345c109dfc 351 */
Ludwigfr 3:37345c109dfc 352 this->map.update_cell_value(i,j,probaLeft);
Ludwigfr 3:37345c109dfc 353 this->map.update_cell_value(i,j,probaFront);
Ludwigfr 3:37345c109dfc 354 this->map.update_cell_value(i,j,probaRight);
Ludwigfr 0:9f7ee7ed13e4 355 }
Ludwigfr 0:9f7ee7ed13e4 356 }
Ludwigfr 0:9f7ee7ed13e4 357 }
Ludwigfr 0:9f7ee7ed13e4 358
Ludwigfr 0:9f7ee7ed13e4 359 void MiniExplorerCoimbra::do_half_flip(){
Ludwigfr 0:9f7ee7ed13e4 360 this->myOdometria();
Ludwigfr 0:9f7ee7ed13e4 361 float theta_plus_h_pi=theta+PI/2;//theta is between -PI and PI
Ludwigfr 0:9f7ee7ed13e4 362 if(theta_plus_h_pi > PI)
Ludwigfr 0:9f7ee7ed13e4 363 theta_plus_h_pi=-(2*PI-theta_plus_h_pi);
Ludwigfr 0:9f7ee7ed13e4 364 leftMotor(0,100);
Ludwigfr 0:9f7ee7ed13e4 365 rightMotor(1,100);
Ludwigfr 0:9f7ee7ed13e4 366 while(abs(theta_plus_h_pi-theta)>0.05){
Ludwigfr 0:9f7ee7ed13e4 367 this->myOdometria();
Ludwigfr 0:9f7ee7ed13e4 368 // pc.printf("\n\r diff=%f", abs(theta_plus_pi-theta));
Ludwigfr 0:9f7ee7ed13e4 369 }
Ludwigfr 0:9f7ee7ed13e4 370 leftMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 371 rightMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 372 }
Ludwigfr 0:9f7ee7ed13e4 373
Ludwigfr 0:9f7ee7ed13e4 374 //Distance computation function
Ludwigfr 0:9f7ee7ed13e4 375 float MiniExplorerCoimbra::dist(float x1, float y1, float x2, float y2){
Ludwigfr 0:9f7ee7ed13e4 376 return sqrt(pow(y2-y1,2) + pow(x2-x1,2));
Ludwigfr 0:9f7ee7ed13e4 377 }
Ludwigfr 0:9f7ee7ed13e4 378
Ludwigfr 0:9f7ee7ed13e4 379 //use virtual force field
Ludwigfr 0:9f7ee7ed13e4 380 void MiniExplorerCoimbra::try_to_reach_target(float targetXWorld,float targetYWorld){
Ludwigfr 0:9f7ee7ed13e4 381 //atan2 gives the angle beetween PI and -PI
Ludwigfr 0:9f7ee7ed13e4 382 this->myOdometria();
Ludwigfr 0:9f7ee7ed13e4 383 /*
Ludwigfr 0:9f7ee7ed13e4 384 float deplacementOnXWorld=targetXWorld-this->xWorld;
Ludwigfr 0:9f7ee7ed13e4 385 float deplacementOnYWorld=targetYWorld-this->yWorld;
Ludwigfr 0:9f7ee7ed13e4 386 */
Ludwigfr 3:37345c109dfc 387 //float angleToTarget=atan2(targetYWorld-this->yWorld,targetXWorld-this->xWorld);
Ludwigfr 3:37345c109dfc 388 //pc.printf("\n angleToTarget=%f",angleToTarget);
Ludwigfr 3:37345c109dfc 389 //turn_to_target(angleToTarget);
Ludwigfr 3:37345c109dfc 390 //TODO IDEA check if maybe set a low max range
Ludwigfr 3:37345c109dfc 391 //this->sonarLeft.setMaxRange(30);
Ludwigfr 3:37345c109dfc 392 //this->sonarFront.setMaxRange(30);
Ludwigfr 3:37345c109dfc 393 //this->sonarRight.setMaxRange(30);
Ludwigfr 0:9f7ee7ed13e4 394 bool reached=false;
Ludwigfr 0:9f7ee7ed13e4 395 int print=0;
Ludwigfr 3:37345c109dfc 396 int printLimit=1000;
Ludwigfr 0:9f7ee7ed13e4 397 while (!reached) {
Ludwigfr 0:9f7ee7ed13e4 398 this->vff(&reached,targetXWorld,targetYWorld);
Ludwigfr 0:9f7ee7ed13e4 399 //test_got_to_line(&reached);
Ludwigfr 3:37345c109dfc 400 if(print==printLimit){
Ludwigfr 0:9f7ee7ed13e4 401 leftMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 402 rightMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 403 this->print_map_with_robot_position_and_target(targetXWorld,targetYWorld);
Ludwigfr 0:9f7ee7ed13e4 404 print=0;
Ludwigfr 0:9f7ee7ed13e4 405 }else
Ludwigfr 0:9f7ee7ed13e4 406 print+=1;
Ludwigfr 0:9f7ee7ed13e4 407 }
Ludwigfr 0:9f7ee7ed13e4 408 //Stop at the end
Ludwigfr 0:9f7ee7ed13e4 409 leftMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 410 rightMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 411 pc.printf("\r\n target reached");
Ludwigfr 5:19f24c363418 412 //wait(3);//
Ludwigfr 0:9f7ee7ed13e4 413 }
Ludwigfr 0:9f7ee7ed13e4 414
Ludwigfr 0:9f7ee7ed13e4 415 void MiniExplorerCoimbra::vff(bool* reached, float targetXWorld, float targetYWorld){
Ludwigfr 0:9f7ee7ed13e4 416 float line_a;
Ludwigfr 0:9f7ee7ed13e4 417 float line_b;
Ludwigfr 0:9f7ee7ed13e4 418 float line_c;
Ludwigfr 0:9f7ee7ed13e4 419 //Updating X,Y and theta with the odometry values
Ludwigfr 0:9f7ee7ed13e4 420 float forceXWorld=0;
Ludwigfr 0:9f7ee7ed13e4 421 float forceYWorld=0;
Ludwigfr 0:9f7ee7ed13e4 422 //we update the odometrie
Ludwigfr 0:9f7ee7ed13e4 423 this->myOdometria();
Ludwigfr 0:9f7ee7ed13e4 424 //we check the sensors
Ludwigfr 0:9f7ee7ed13e4 425 float leftMm = get_distance_left_sensor();
Ludwigfr 0:9f7ee7ed13e4 426 float frontMm = get_distance_front_sensor();
Ludwigfr 0:9f7ee7ed13e4 427 float rightMm = get_distance_right_sensor();
Ludwigfr 0:9f7ee7ed13e4 428 //update the probabilities values
Ludwigfr 0:9f7ee7ed13e4 429 this->update_sonar_values(leftMm, frontMm, rightMm);
Ludwigfr 8:072a76960e27 430 this->myOdometria();
Ludwigfr 0:9f7ee7ed13e4 431 //we compute the force on X and Y
Ludwigfr 0:9f7ee7ed13e4 432 this->compute_forceX_and_forceY(&forceXWorld, &forceYWorld,targetXWorld,targetYWorld);
Ludwigfr 0:9f7ee7ed13e4 433 //we compute a new ine
Ludwigfr 0:9f7ee7ed13e4 434 this->calculate_line(forceXWorld, forceYWorld, &line_a,&line_b,&line_c);
Ludwigfr 0:9f7ee7ed13e4 435 //Updating motor velocities
Ludwigfr 8:072a76960e27 436 //pc.printf("\r\nX=%f;Y=%f",xWorld,yWorld);
Ludwigfr 8:072a76960e27 437 //pc.printf("\r\n%f*x+%f*y+%f=0",line_a,line_b,line_c);
Ludwigfr 0:9f7ee7ed13e4 438 this->go_to_line(line_a,line_b,line_c,targetXWorld,targetYWorld);
Ludwigfr 3:37345c109dfc 439
Ludwigfr 0:9f7ee7ed13e4 440 //wait(0.1);
Ludwigfr 0:9f7ee7ed13e4 441 this->myOdometria();
Ludwigfr 3:37345c109dfc 442 if(dist(this->xWorld,this->yWorld,targetXWorld,targetYWorld)<3)
Ludwigfr 0:9f7ee7ed13e4 443 *reached=true;
Ludwigfr 0:9f7ee7ed13e4 444 }
Ludwigfr 0:9f7ee7ed13e4 445
Ludwigfr 0:9f7ee7ed13e4 446 /*angleToTarget is obtained through atan2 so it s:
Ludwigfr 0:9f7ee7ed13e4 447 < 0 if the angle is bettween PI and 2pi on a trigo circle
Ludwigfr 0:9f7ee7ed13e4 448 > 0 if it is between 0 and PI
Ludwigfr 0:9f7ee7ed13e4 449 */
Ludwigfr 0:9f7ee7ed13e4 450 void MiniExplorerCoimbra::turn_to_target(float angleToTarget){
Ludwigfr 0:9f7ee7ed13e4 451 this->myOdometria();
Ludwigfr 3:37345c109dfc 452 if(angleToTarget!=0){
Ludwigfr 3:37345c109dfc 453 if(angleToTarget>0){
Ludwigfr 3:37345c109dfc 454 leftMotor(0,1);
Ludwigfr 3:37345c109dfc 455 rightMotor(1,1);
Ludwigfr 3:37345c109dfc 456 }else{
Ludwigfr 3:37345c109dfc 457 leftMotor(1,1);
Ludwigfr 3:37345c109dfc 458 rightMotor(0,1);
Ludwigfr 3:37345c109dfc 459 }
Ludwigfr 3:37345c109dfc 460 while(abs(angleToTarget-this->thetaWorld)>0.05)
Ludwigfr 3:37345c109dfc 461 this->myOdometria();
Ludwigfr 0:9f7ee7ed13e4 462 }
Ludwigfr 0:9f7ee7ed13e4 463 leftMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 464 rightMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 465 }
Ludwigfr 0:9f7ee7ed13e4 466
Ludwigfr 0:9f7ee7ed13e4 467
Ludwigfr 0:9f7ee7ed13e4 468 void MiniExplorerCoimbra::print_map_with_robot_position_and_target(float targetXWorld, float targetYWorld) {
Ludwigfr 0:9f7ee7ed13e4 469 float currProba;
Ludwigfr 0:9f7ee7ed13e4 470
Ludwigfr 0:9f7ee7ed13e4 471 float heightIndiceInOrthonormal;
Ludwigfr 0:9f7ee7ed13e4 472 float widthIndiceInOrthonormal;
Ludwigfr 0:9f7ee7ed13e4 473
Ludwigfr 0:9f7ee7ed13e4 474 float widthMalus=-(3*this->map.sizeCellWidth/2);
Ludwigfr 0:9f7ee7ed13e4 475 float widthBonus=this->map.sizeCellWidth/2;
Ludwigfr 0:9f7ee7ed13e4 476
Ludwigfr 0:9f7ee7ed13e4 477 float heightMalus=-(3*this->map.sizeCellHeight/2);
Ludwigfr 0:9f7ee7ed13e4 478 float heightBonus=this->map.sizeCellHeight/2;
Ludwigfr 0:9f7ee7ed13e4 479
Ludwigfr 0:9f7ee7ed13e4 480 pc.printf("\n\r");
Ludwigfr 0:9f7ee7ed13e4 481 for (int y = this->map.nbCellHeight -1; y>-1; y--) {
Ludwigfr 0:9f7ee7ed13e4 482 for (int x= 0; x<this->map.nbCellWidth; x++) {
Ludwigfr 0:9f7ee7ed13e4 483 heightIndiceInOrthonormal=this->map.cell_height_coordinate_to_world(y);
Ludwigfr 0:9f7ee7ed13e4 484 widthIndiceInOrthonormal=this->map.cell_width_coordinate_to_world(x);
Ludwigfr 0:9f7ee7ed13e4 485 if(this->yWorld >= (heightIndiceInOrthonormal+heightMalus) && this->yWorld <= (heightIndiceInOrthonormal+heightBonus) && this->xWorld >= (widthIndiceInOrthonormal+widthMalus) && this->xWorld <= (widthIndiceInOrthonormal+widthBonus))
Ludwigfr 0:9f7ee7ed13e4 486 pc.printf(" R ");
Ludwigfr 0:9f7ee7ed13e4 487 else{
Ludwigfr 0:9f7ee7ed13e4 488 if(targetYWorld >= (heightIndiceInOrthonormal+heightMalus) && targetYWorld <= (heightIndiceInOrthonormal+heightBonus) && targetXWorld >= (widthIndiceInOrthonormal+widthMalus) && targetXWorld <= (widthIndiceInOrthonormal+widthBonus))
Ludwigfr 0:9f7ee7ed13e4 489 pc.printf(" T ");
Ludwigfr 0:9f7ee7ed13e4 490 else{
Ludwigfr 0:9f7ee7ed13e4 491 currProba=this->map.log_to_proba(this->map.cellsLogValues[x][y]);
Ludwigfr 5:19f24c363418 492 if ( currProba < 0.5){
Ludwigfr 0:9f7ee7ed13e4 493 pc.printf(" ");
Ludwigfr 5:19f24c363418 494 //pc.printf("%f",currProba);
Ludwigfr 5:19f24c363418 495 }else{
Ludwigfr 5:19f24c363418 496 if(currProba==0.5){
Ludwigfr 0:9f7ee7ed13e4 497 pc.printf(" . ");
Ludwigfr 5:19f24c363418 498 //pc.printf("%f",currProba);
Ludwigfr 5:19f24c363418 499 }else{
Ludwigfr 0:9f7ee7ed13e4 500 pc.printf(" X ");
Ludwigfr 5:19f24c363418 501 //pc.printf("%f",currProba);
Ludwigfr 5:19f24c363418 502 }
Ludwigfr 0:9f7ee7ed13e4 503 }
Ludwigfr 0:9f7ee7ed13e4 504 }
Ludwigfr 0:9f7ee7ed13e4 505 }
Ludwigfr 0:9f7ee7ed13e4 506 }
Ludwigfr 0:9f7ee7ed13e4 507 pc.printf("\n\r");
Ludwigfr 0:9f7ee7ed13e4 508 }
Ludwigfr 0:9f7ee7ed13e4 509 }
Ludwigfr 0:9f7ee7ed13e4 510
Ludwigfr 2:11cd5173aa36 511 void MiniExplorerCoimbra::print_map_with_robot_position(){
Ludwigfr 2:11cd5173aa36 512 float currProba;
Ludwigfr 2:11cd5173aa36 513
Ludwigfr 2:11cd5173aa36 514 float heightIndiceInOrthonormal;
Ludwigfr 2:11cd5173aa36 515 float widthIndiceInOrthonormal;
Ludwigfr 2:11cd5173aa36 516
Ludwigfr 2:11cd5173aa36 517 float widthMalus=-(3*this->map.sizeCellWidth/2);
Ludwigfr 2:11cd5173aa36 518 float widthBonus=this->map.sizeCellWidth/2;
Ludwigfr 2:11cd5173aa36 519
Ludwigfr 2:11cd5173aa36 520 float heightMalus=-(3*this->map.sizeCellHeight/2);
Ludwigfr 2:11cd5173aa36 521 float heightBonus=this->map.sizeCellHeight/2;
Ludwigfr 2:11cd5173aa36 522
Ludwigfr 2:11cd5173aa36 523 pc.printf("\n\r");
Ludwigfr 2:11cd5173aa36 524 for (int y = this->map.nbCellHeight -1; y>-1; y--) {
Ludwigfr 2:11cd5173aa36 525 for (int x= 0; x<this->map.nbCellWidth; x++) {
Ludwigfr 2:11cd5173aa36 526 heightIndiceInOrthonormal=this->map.cell_height_coordinate_to_world(y);
Ludwigfr 2:11cd5173aa36 527 widthIndiceInOrthonormal=this->map.cell_width_coordinate_to_world(x);
Ludwigfr 5:19f24c363418 528 if(this->yWorld >= (heightIndiceInOrthonormal+heightMalus) && this->yWorld <= (heightIndiceInOrthonormal+heightBonus) && this->xWorld >= (widthIndiceInOrthonormal+widthMalus) && this->xWorld <= (widthIndiceInOrthonormal+widthBonus)){
Ludwigfr 2:11cd5173aa36 529 pc.printf(" R ");
Ludwigfr 5:19f24c363418 530 //pc.printf("%f",currProba);
Ludwigfr 5:19f24c363418 531 }else{
Ludwigfr 2:11cd5173aa36 532 currProba=this->map.log_to_proba(this->map.cellsLogValues[x][y]);
Ludwigfr 5:19f24c363418 533 if ( currProba < 0.5){
Ludwigfr 2:11cd5173aa36 534 pc.printf(" ");
Ludwigfr 5:19f24c363418 535 //pc.printf("%f",currProba);
Ludwigfr 5:19f24c363418 536 }else{
Ludwigfr 5:19f24c363418 537 if(currProba==0.5){
Ludwigfr 2:11cd5173aa36 538 pc.printf(" . ");
Ludwigfr 5:19f24c363418 539 //pc.printf("%f",currProba);
Ludwigfr 5:19f24c363418 540 }else{
Ludwigfr 5:19f24c363418 541 pc.printf(" X ");
Ludwigfr 5:19f24c363418 542 //pc.printf("%f",currProba);
Ludwigfr 5:19f24c363418 543 }
Ludwigfr 2:11cd5173aa36 544 }
Ludwigfr 2:11cd5173aa36 545 }
Ludwigfr 2:11cd5173aa36 546 }
Ludwigfr 2:11cd5173aa36 547 pc.printf("\n\r");
Ludwigfr 2:11cd5173aa36 548 }
Ludwigfr 2:11cd5173aa36 549 }
Ludwigfr 0:9f7ee7ed13e4 550
Ludwigfr 0:9f7ee7ed13e4 551 //robotX and robotY are from this->myOdometria(), calculate line_a, line_b and line_c
Ludwigfr 0:9f7ee7ed13e4 552 void MiniExplorerCoimbra::calculate_line(float forceX, float forceY, float *line_a, float *line_b, float *line_c){
Ludwigfr 0:9f7ee7ed13e4 553 /*
Ludwigfr 0:9f7ee7ed13e4 554 in the teachers maths it is
Ludwigfr 0:9f7ee7ed13e4 555
Ludwigfr 0:9f7ee7ed13e4 556 *line_a=forceY;
Ludwigfr 0:9f7ee7ed13e4 557 *line_b=-forceX;
Ludwigfr 0:9f7ee7ed13e4 558
Ludwigfr 0:9f7ee7ed13e4 559 because a*x+b*y+c=0
Ludwigfr 0:9f7ee7ed13e4 560 a impact the vertical and b the horizontal
Ludwigfr 0:9f7ee7ed13e4 561 and he has to put them like this because
Ludwigfr 0:9f7ee7ed13e4 562 Robot space: World space:
Ludwigfr 0:9f7ee7ed13e4 563 ^ ^
Ludwigfr 0:9f7ee7ed13e4 564 |x |y
Ludwigfr 0:9f7ee7ed13e4 565 <- R O ->
Ludwigfr 0:9f7ee7ed13e4 566 y x
Ludwigfr 0:9f7ee7ed13e4 567 but since our forceX, forceY are already computed in the orthonormal space I m not sure we need to
Ludwigfr 0:9f7ee7ed13e4 568 */
Ludwigfr 3:37345c109dfc 569 //*line_a=forceX;
Ludwigfr 3:37345c109dfc 570 //*line_b=forceY;
Ludwigfr 3:37345c109dfc 571
Ludwigfr 3:37345c109dfc 572 *line_a=forceY;
Ludwigfr 3:37345c109dfc 573 *line_b=-forceX;
Ludwigfr 0:9f7ee7ed13e4 574 //because the line computed always pass by the robot center we dont need lince_c
Ludwigfr 8:072a76960e27 575 *line_c=forceX*this->yWorld+forceY*this->xWorld;
Ludwigfr 8:072a76960e27 576 //*line_c=0;
Ludwigfr 0:9f7ee7ed13e4 577 }
Ludwigfr 0:9f7ee7ed13e4 578 //compute the force on X and Y
Ludwigfr 0:9f7ee7ed13e4 579 void MiniExplorerCoimbra::compute_forceX_and_forceY(float* forceXWorld, float* forceYWorld, float targetXWorld, float targetYWorld){
Ludwigfr 0:9f7ee7ed13e4 580 float forceRepulsionComputedX=0;
Ludwigfr 0:9f7ee7ed13e4 581 float forceRepulsionComputedY=0;
Ludwigfr 8:072a76960e27 582 this->print_map_with_robot_position();
Ludwigfr 8:072a76960e27 583 for(int i=0;i<this->map.nbCellWidth;i++){ //for each cell of the map we compute a force of repulsion
Ludwigfr 0:9f7ee7ed13e4 584 for(int j=0;j<this->map.nbCellHeight;j++){
Ludwigfr 0:9f7ee7ed13e4 585 this->update_force(i,j,&forceRepulsionComputedX,&forceRepulsionComputedY);
Ludwigfr 0:9f7ee7ed13e4 586 }
Ludwigfr 0:9f7ee7ed13e4 587 }
Ludwigfr 0:9f7ee7ed13e4 588 //update with attraction force
Ludwigfr 3:37345c109dfc 589 *forceXWorld=forceRepulsionComputedX;
Ludwigfr 3:37345c109dfc 590 *forceYWorld=forceRepulsionComputedY;
Ludwigfr 8:072a76960e27 591 //this->print_map_with_robot_position();
Ludwigfr 8:072a76960e27 592 //pc.printf("\r\nForce X repul:%f",*forceXWorld);
Ludwigfr 8:072a76960e27 593 //pc.printf("\r\nForce Y repul:%f",*forceYWorld);
Ludwigfr 9:1cc27f33d3e1 594
Ludwigfr 9:1cc27f33d3e1 595 //test without atraction being impacted by distance
Ludwigfr 9:1cc27f33d3e1 596 //*forceXWorld+=this->attractionConstantForce*(targetXWorld-this->xWorld);
Ludwigfr 9:1cc27f33d3e1 597 //*forceYWorld+=this->attractionConstantForce*(targetYWorld-this->yWorld);
Ludwigfr 9:1cc27f33d3e1 598
Ludwigfr 0:9f7ee7ed13e4 599 float distanceTargetRobot=sqrt(pow(targetXWorld-this->xWorld,2)+pow(targetYWorld-this->yWorld,2));
Ludwigfr 0:9f7ee7ed13e4 600 if(distanceTargetRobot != 0){
Ludwigfr 8:072a76960e27 601 *forceXWorld+=this->attractionConstantForce*(targetXWorld-this->xWorld)/distanceTargetRobot;
Ludwigfr 8:072a76960e27 602 *forceYWorld+=this->attractionConstantForce*(targetYWorld-this->yWorld)/distanceTargetRobot;
Ludwigfr 3:37345c109dfc 603 }else{
Ludwigfr 8:072a76960e27 604 *forceXWorld+=this->attractionConstantForce*(targetXWorld-this->xWorld)/0.001;
Ludwigfr 8:072a76960e27 605 *forceYWorld+=this->attractionConstantForce*(targetYWorld-this->yWorld)/0.001;
Ludwigfr 0:9f7ee7ed13e4 606 }
Ludwigfr 9:1cc27f33d3e1 607
Ludwigfr 8:072a76960e27 608 //pc.printf("\r\nForce X after attract:%f",*forceXWorld);
Ludwigfr 8:072a76960e27 609 //pc.printf("\r\nForce Y after attract:%f",*forceYWorld);
Ludwigfr 3:37345c109dfc 610
Ludwigfr 0:9f7ee7ed13e4 611 float amplitude=sqrt(pow(*forceXWorld,2)+pow(*forceYWorld,2));
Ludwigfr 0:9f7ee7ed13e4 612 if(amplitude!=0){//avoid division by 0 if forceX and forceY == 0
Ludwigfr 0:9f7ee7ed13e4 613 *forceXWorld=*forceXWorld/amplitude;
Ludwigfr 0:9f7ee7ed13e4 614 *forceYWorld=*forceYWorld/amplitude;
Ludwigfr 3:37345c109dfc 615 }else{
Ludwigfr 8:072a76960e27 616 *forceXWorld=*forceXWorld/0.001;
Ludwigfr 8:072a76960e27 617 *forceYWorld=*forceYWorld/0.001;
Ludwigfr 0:9f7ee7ed13e4 618 }
Ludwigfr 0:9f7ee7ed13e4 619 }
Ludwigfr 0:9f7ee7ed13e4 620
Ludwigfr 2:11cd5173aa36 621 //for vff
Ludwigfr 0:9f7ee7ed13e4 622 void MiniExplorerCoimbra::go_to_line(float line_a, float line_b, float line_c,float targetXWorld, float targetYWorld){
Ludwigfr 0:9f7ee7ed13e4 623 float lineAngle;
Ludwigfr 0:9f7ee7ed13e4 624 float angleError;
Ludwigfr 0:9f7ee7ed13e4 625 float linear;
Ludwigfr 0:9f7ee7ed13e4 626 float angular;
Ludwigfr 2:11cd5173aa36 627 float d;
Ludwigfr 0:9f7ee7ed13e4 628
Ludwigfr 0:9f7ee7ed13e4 629 //line angle is beetween pi/2 and -pi/2
Ludwigfr 2:11cd5173aa36 630
Ludwigfr 2:11cd5173aa36 631 if(line_b!=0){
Ludwigfr 2:11cd5173aa36 632 lineAngle=atan(line_a/-line_b);
Ludwigfr 0:9f7ee7ed13e4 633 }
Ludwigfr 0:9f7ee7ed13e4 634 else{
Ludwigfr 3:37345c109dfc 635 lineAngle=0;
Ludwigfr 0:9f7ee7ed13e4 636 }
Ludwigfr 0:9f7ee7ed13e4 637
Ludwigfr 2:11cd5173aa36 638 this->myOdometria();
Ludwigfr 2:11cd5173aa36 639 //Computing angle error
Ludwigfr 2:11cd5173aa36 640 angleError = lineAngle-this->thetaWorld;//TODO that I m not sure
Ludwigfr 2:11cd5173aa36 641 if(angleError>PI)
Ludwigfr 2:11cd5173aa36 642 angleError=-(angleError-PI);
Ludwigfr 2:11cd5173aa36 643 else
Ludwigfr 2:11cd5173aa36 644 if(angleError<-PI)
Ludwigfr 2:11cd5173aa36 645 angleError=-(angleError+PI);
Ludwigfr 2:11cd5173aa36 646
Ludwigfr 3:37345c109dfc 647 //d=this->distFromLine(this->xWorld, this->yWorld, line_a, line_b, line_c);//this could be 0
Ludwigfr 3:37345c109dfc 648 d=0;
Ludwigfr 8:072a76960e27 649 //pc.printf("\r\ndistance from line:%f",d);
Ludwigfr 0:9f7ee7ed13e4 650 //Calculating velocities
Ludwigfr 2:11cd5173aa36 651 linear= this->kv*(3.14);
Ludwigfr 2:11cd5173aa36 652 angular=-this->kd*d + this->kh*angleError;
Ludwigfr 0:9f7ee7ed13e4 653
Ludwigfr 0:9f7ee7ed13e4 654 float angularLeft=(linear-0.5*this->distanceWheels*angular)/this->radiusWheels;
Ludwigfr 2:11cd5173aa36 655 float angularRight=(linear+0.5*this->distanceWheels*angular)/this->radiusWheels;
Ludwigfr 0:9f7ee7ed13e4 656
Ludwigfr 2:11cd5173aa36 657 //Normalize speed for motors
Ludwigfr 0:9f7ee7ed13e4 658 if(abs(angularLeft)>abs(angularRight)) {
Ludwigfr 0:9f7ee7ed13e4 659 angularRight=this->speed*abs(angularRight/angularLeft)*this->sign1(angularRight);
Ludwigfr 0:9f7ee7ed13e4 660 angularLeft=this->speed*this->sign1(angularLeft);
Ludwigfr 0:9f7ee7ed13e4 661 }
Ludwigfr 0:9f7ee7ed13e4 662 else {
Ludwigfr 0:9f7ee7ed13e4 663 angularLeft=this->speed*abs(angularLeft/angularRight)*this->sign1(angularLeft);
Ludwigfr 0:9f7ee7ed13e4 664 angularRight=this->speed*this->sign1(angularRight);
Ludwigfr 0:9f7ee7ed13e4 665 }
Ludwigfr 2:11cd5173aa36 666
Ludwigfr 2:11cd5173aa36 667 pc.printf("\r\nd = %f", d);
Ludwigfr 3:37345c109dfc 668 pc.printf("\r\nerror = %f, lineAngle=%f, robotAngle=%f\n", angleError,lineAngle,this->thetaWorld);
Ludwigfr 2:11cd5173aa36 669
Ludwigfr 0:9f7ee7ed13e4 670 leftMotor(this->sign2(angularLeft),abs(angularLeft));
Ludwigfr 0:9f7ee7ed13e4 671 rightMotor(this->sign2(angularRight),abs(angularRight));
Ludwigfr 0:9f7ee7ed13e4 672 }
Ludwigfr 0:9f7ee7ed13e4 673
geotsam 1:20f48907c726 674 void MiniExplorerCoimbra::go_to_line_first_lab(float line_a, float line_b, float line_c){
geotsam 1:20f48907c726 675 float lineAngle;
geotsam 1:20f48907c726 676 float angleError;
geotsam 1:20f48907c726 677 float linear;
geotsam 1:20f48907c726 678 float angular;
geotsam 1:20f48907c726 679 float d;
geotsam 1:20f48907c726 680
geotsam 1:20f48907c726 681 //line angle is beetween pi/2 and -pi/2
geotsam 1:20f48907c726 682
geotsam 1:20f48907c726 683 if(line_b!=0){
geotsam 1:20f48907c726 684 lineAngle=atan(line_a/-line_b);
geotsam 1:20f48907c726 685 }
geotsam 1:20f48907c726 686 else{
geotsam 1:20f48907c726 687 lineAngle=1.5708;
geotsam 1:20f48907c726 688 }
geotsam 1:20f48907c726 689
geotsam 1:20f48907c726 690 do{
geotsam 1:20f48907c726 691 this->myOdometria();
geotsam 1:20f48907c726 692 //Computing angle error
Ludwigfr 3:37345c109dfc 693 pc.printf("\r\nline angle = %f", lineAngle);
Ludwigfr 3:37345c109dfc 694 pc.printf("\r\nthetaWorld = %f", thetaWorld);
geotsam 1:20f48907c726 695 angleError = lineAngle-this->thetaWorld;//TODO that I m not sure
geotsam 1:20f48907c726 696
Ludwigfr 3:37345c109dfc 697 if(angleError>PI)
Ludwigfr 3:37345c109dfc 698 angleError=-(angleError-PI);
Ludwigfr 3:37345c109dfc 699 else
Ludwigfr 3:37345c109dfc 700 if(angleError<-PI)
Ludwigfr 3:37345c109dfc 701 angleError=-(angleError+PI);
geotsam 1:20f48907c726 702
Ludwigfr 3:37345c109dfc 703 pc.printf("\r\nangleError = %f\n", angleError);
geotsam 1:20f48907c726 704 d=this->distFromLine(xWorld, yWorld, line_a, line_b, line_c);
Ludwigfr 3:37345c109dfc 705 pc.printf("\r\ndistance to line = %f", d);
geotsam 1:20f48907c726 706
geotsam 1:20f48907c726 707 //Calculating velocities
geotsam 1:20f48907c726 708 linear= this->kv*(3.14);
geotsam 1:20f48907c726 709 angular=-this->kd*d + this->kh*angleError;
geotsam 1:20f48907c726 710
geotsam 1:20f48907c726 711 float angularLeft=(linear-0.5*this->distanceWheels*angular)/this->radiusWheels;
geotsam 1:20f48907c726 712 float angularRight=(linear+0.5*this->distanceWheels*angular)/this->radiusWheels;
geotsam 1:20f48907c726 713
geotsam 1:20f48907c726 714 //Normalize speed for motors
geotsam 1:20f48907c726 715 if(abs(angularLeft)>abs(angularRight)) {
geotsam 1:20f48907c726 716 angularRight=this->speed*abs(angularRight/angularLeft)*this->sign1(angularRight);
geotsam 1:20f48907c726 717 angularLeft=this->speed*this->sign1(angularLeft);
geotsam 1:20f48907c726 718 }
geotsam 1:20f48907c726 719 else {
geotsam 1:20f48907c726 720 angularLeft=this->speed*abs(angularLeft/angularRight)*this->sign1(angularLeft);
geotsam 1:20f48907c726 721 angularRight=this->speed*this->sign1(angularRight);
geotsam 1:20f48907c726 722 }
geotsam 1:20f48907c726 723
geotsam 1:20f48907c726 724 leftMotor(this->sign2(angularLeft),abs(angularLeft));
geotsam 1:20f48907c726 725 rightMotor(this->sign2(angularRight),abs(angularRight));
geotsam 1:20f48907c726 726 }while(1);
geotsam 1:20f48907c726 727 }
geotsam 1:20f48907c726 728
Ludwigfr 0:9f7ee7ed13e4 729 void MiniExplorerCoimbra::update_force(int widthIndice, int heightIndice, float* forceRepulsionComputedX, float* forceRepulsionComputedY ){
Ludwigfr 0:9f7ee7ed13e4 730 //get the coordonate of the map and the robot in the ortonormal space
Ludwigfr 0:9f7ee7ed13e4 731 float xWorldCell=this->map.cell_width_coordinate_to_world(widthIndice);
Ludwigfr 0:9f7ee7ed13e4 732 float yWorldCell=this->map.cell_height_coordinate_to_world(heightIndice);
Ludwigfr 0:9f7ee7ed13e4 733 //compute the distance beetween the cell and the robot
Ludwigfr 0:9f7ee7ed13e4 734 float distanceCellToRobot=sqrt(pow(xWorldCell-this->xWorld,2)+pow(yWorldCell-this->yWorld,2));
Ludwigfr 3:37345c109dfc 735 float probaCell;
Ludwigfr 0:9f7ee7ed13e4 736 //check if the cell is in range
Ludwigfr 8:072a76960e27 737 //float anglePointToRobot=atan2(yWorldCell-this->yWorld,xWorldCell-this->xWorld);//like world system
Ludwigfr 3:37345c109dfc 738 float temp1;
Ludwigfr 3:37345c109dfc 739 float temp2;
Ludwigfr 0:9f7ee7ed13e4 740 if(distanceCellToRobot <= this->rangeForce) {
Ludwigfr 3:37345c109dfc 741 probaCell=this->map.get_proba_cell(widthIndice,heightIndice);
Ludwigfr 8:072a76960e27 742 //pc.printf("\r\nupdate force proba:%f",probaCell);
Ludwigfr 3:37345c109dfc 743 temp1=this->repulsionConstantForce*probaCell/pow(distanceCellToRobot,2);
Ludwigfr 3:37345c109dfc 744 temp2=(xWorldCell-this->xWorld)/distanceCellToRobot;
Ludwigfr 3:37345c109dfc 745 *forceRepulsionComputedX+=temp1*temp2;
Ludwigfr 3:37345c109dfc 746 temp2=(yWorldCell-this->yWorld)/distanceCellToRobot;
Ludwigfr 3:37345c109dfc 747 *forceRepulsionComputedY+=temp1*temp2;
Ludwigfr 0:9f7ee7ed13e4 748 }
Ludwigfr 0:9f7ee7ed13e4 749 }
Ludwigfr 0:9f7ee7ed13e4 750
Ludwigfr 0:9f7ee7ed13e4 751 //return 1 if positiv, -1 if negativ
Ludwigfr 0:9f7ee7ed13e4 752 float MiniExplorerCoimbra::sign1(float value){
Ludwigfr 0:9f7ee7ed13e4 753 if(value>=0)
Ludwigfr 0:9f7ee7ed13e4 754 return 1;
Ludwigfr 0:9f7ee7ed13e4 755 else
Ludwigfr 0:9f7ee7ed13e4 756 return -1;
Ludwigfr 0:9f7ee7ed13e4 757 }
Ludwigfr 0:9f7ee7ed13e4 758
Ludwigfr 0:9f7ee7ed13e4 759 //return 1 if positiv, 0 if negativ
Ludwigfr 0:9f7ee7ed13e4 760 int MiniExplorerCoimbra::sign2(float value){
Ludwigfr 0:9f7ee7ed13e4 761 if(value>=0)
Ludwigfr 0:9f7ee7ed13e4 762 return 1;
Ludwigfr 0:9f7ee7ed13e4 763 else
Ludwigfr 0:9f7ee7ed13e4 764 return 0;
Ludwigfr 0:9f7ee7ed13e4 765 }
Ludwigfr 0:9f7ee7ed13e4 766
geotsam 1:20f48907c726 767 float MiniExplorerCoimbra::distFromLine(float robot_x, float robot_y, float line_a, float line_b, float line_c){
geotsam 1:20f48907c726 768 return abs((line_a*robot_x+line_b*robot_y+line_c)/sqrt(line_a*line_a+line_b*line_b));
geotsam 1:20f48907c726 769 }
geotsam 1:20f48907c726 770
Ludwigfr 5:19f24c363418 771 //4th LAB
Ludwigfr 5:19f24c363418 772 //starting position lower left
Ludwigfr 5:19f24c363418 773
Ludwigfr 10:d0109d7cbe7c 774 void MiniExplorerCoimbra::test_procedure_lab_4(float sizeX, float sizeY){
Ludwigfr 5:19f24c363418 775
Ludwigfr 5:19f24c363418 776 this->map.fill_map_with_kalman_knowledge();
Ludwigfr 14:696187e74411 777 this->go_to_point_kalman2(this->xWorld+sizeX,this->yWorld+sizeY);
Ludwigfr 14:696187e74411 778 }
Ludwigfr 14:696187e74411 779
Ludwigfr 14:696187e74411 780 void MiniExplorerCoimbra::myOdometriaBis(){
Ludwigfr 14:696187e74411 781 float R,L;
Ludwigfr 14:696187e74411 782 this->testOdometria(&R,&L);
Ludwigfr 14:696187e74411 783
Ludwigfr 14:696187e74411 784 this->xWorld=-Y;
Ludwigfr 14:696187e74411 785 this->yWorld=X;
Ludwigfr 14:696187e74411 786 if(theta >PI/2)
Ludwigfr 14:696187e74411 787 this->thetaWorld=-PI+(theta-PI/2);
Ludwigfr 14:696187e74411 788 else
Ludwigfr 14:696187e74411 789 this->thetaWorld=theta+PI/2;
Ludwigfr 5:19f24c363418 790 }
Ludwigfr 5:19f24c363418 791
Ludwigfr 14:696187e74411 792 void MiniExplorerCoimbra::go_to_point_kalman(float targetXWorld, float targetYWorld) {
Ludwigfr 14:696187e74411 793
Ludwigfr 14:696187e74411 794 float angleError; //angle error
Ludwigfr 14:696187e74411 795 float d; //distance from target
Ludwigfr 14:696187e74411 796 float angularLeft, angularRight, linear, angular;
Ludwigfr 14:696187e74411 797 int speed=300;
Ludwigfr 14:696187e74411 798
Ludwigfr 14:696187e74411 799 do {
Ludwigfr 14:696187e74411 800 //Updating X,Y and theta with the odometry values
Ludwigfr 14:696187e74411 801 this->OdometriaKalmanFilter();
Ludwigfr 14:696187e74411 802 pc.printf("\n\r X=%f", this->xWorld);
Ludwigfr 14:696187e74411 803 pc.printf("\n\r Y=%f", this->yWorld);
Ludwigfr 14:696187e74411 804 pc.printf("\n\r theta=%f", this->thetaWorld);
Ludwigfr 14:696187e74411 805 //Computing angle error and distance towards the target value
Ludwigfr 14:696187e74411 806 angleError = atan2((targetYWorld-this->yWorld),(targetXWorld-this->xWorld))-this->thetaWorld;
Ludwigfr 14:696187e74411 807 if(angleError>PI) angleError=-(angleError-PI);
Ludwigfr 14:696187e74411 808 else if(angleError<-PI) angleError=-(angleError+PI);
Ludwigfr 14:696187e74411 809 //pc.printf("\n\r error=%f",angleError);
Ludwigfr 14:696187e74411 810
Ludwigfr 14:696187e74411 811 d=this->dist(this->xWorld, this->yWorld, targetXWorld, targetYWorld);
Ludwigfr 14:696187e74411 812 //pc.printf("\n\r dist=%f/n", d);
Ludwigfr 14:696187e74411 813
Ludwigfr 14:696187e74411 814 //Computing linear and angular velocities
Ludwigfr 14:696187e74411 815 linear=k_linear*d;
Ludwigfr 14:696187e74411 816 angular=k_angular*angleError;
Ludwigfr 14:696187e74411 817 angularLeft=(linear-0.5*this->distanceWheels*angular)/this->radiusWheels;
Ludwigfr 14:696187e74411 818 angularRight=(linear+0.5*this->distanceWheels*angular)/this->radiusWheels;
Ludwigfr 5:19f24c363418 819
Ludwigfr 14:696187e74411 820
Ludwigfr 14:696187e74411 821 //Normalize speed for motors
Ludwigfr 14:696187e74411 822 if(angularLeft>angularRight) {
Ludwigfr 14:696187e74411 823 angularRight=speed*angularRight/angularLeft;
Ludwigfr 14:696187e74411 824 angularLeft=speed;
Ludwigfr 14:696187e74411 825 } else {
Ludwigfr 14:696187e74411 826 angularLeft=speed*angularLeft/angularRight;
Ludwigfr 14:696187e74411 827 angularRight=speed;
Ludwigfr 14:696187e74411 828 }
Ludwigfr 5:19f24c363418 829
Ludwigfr 14:696187e74411 830 //Updating motor velocities
Ludwigfr 14:696187e74411 831 if(angularLeft>0){
Ludwigfr 14:696187e74411 832 leftMotor(1,angularLeft);
Ludwigfr 14:696187e74411 833 }
Ludwigfr 14:696187e74411 834 else{
Ludwigfr 14:696187e74411 835 leftMotor(0,-angularLeft);
Ludwigfr 14:696187e74411 836 }
Ludwigfr 14:696187e74411 837
Ludwigfr 14:696187e74411 838 if(angularRight>0){
Ludwigfr 14:696187e74411 839 rightMotor(1,angularRight);
Ludwigfr 14:696187e74411 840 }
Ludwigfr 14:696187e74411 841 else{
Ludwigfr 14:696187e74411 842 rightMotor(0,-angularRight);
Ludwigfr 14:696187e74411 843 }
Ludwigfr 14:696187e74411 844
Ludwigfr 14:696187e74411 845 //wait(0.5);
Ludwigfr 14:696187e74411 846 } while(d>1);
Ludwigfr 14:696187e74411 847 pc.printf("\r\n REACHED");
Ludwigfr 5:19f24c363418 848 //Stop at the end
Ludwigfr 5:19f24c363418 849 leftMotor(1,0);
Ludwigfr 5:19f24c363418 850 rightMotor(1,0);
Ludwigfr 5:19f24c363418 851 }
Ludwigfr 5:19f24c363418 852
Ludwigfr 14:696187e74411 853 void MiniExplorerCoimbra::go_to_point_kalman2(float targetXWorld, float targetYWorld) {
Ludwigfr 14:696187e74411 854 //float d; //distance from target
Ludwigfr 14:696187e74411 855 float angularLeft, angularRight;
Ludwigfr 14:696187e74411 856 float correctX;
Ludwigfr 14:696187e74411 857 float correctY;
Ludwigfr 14:696187e74411 858 float correctTheta;
Ludwigfr 14:696187e74411 859 float xAtEnd=targetXWorld;
Ludwigfr 14:696187e74411 860 do {
Ludwigfr 14:696187e74411 861 //Updating X,Y and theta with the odometry values
Ludwigfr 14:696187e74411 862 this->OdometriaKalmanFilter();
Ludwigfr 14:696187e74411 863
Ludwigfr 14:696187e74411 864 correctX=-this->myY_r_correct;
Ludwigfr 14:696187e74411 865 correctY=this->myX_r_correct;
Ludwigfr 14:696187e74411 866 if(this->mytheta_r_correct >PI/2)
Ludwigfr 14:696187e74411 867 correctTheta=-PI+(this->mytheta_r_correct-PI/2);
Ludwigfr 14:696187e74411 868 else
Ludwigfr 14:696187e74411 869 correctTheta=this->mytheta_r_correct+PI/2;
Ludwigfr 14:696187e74411 870
Ludwigfr 14:696187e74411 871 pc.printf("\n\rFalse: X=%f,Y=%f,theta=%f", this->xWorld,this->yWorld,this->thetaWorld);
Ludwigfr 14:696187e74411 872 pc.printf("\n\rTrue : X=%f,Y=%f,theta=%f", correctX,correctY,correctTheta);
Ludwigfr 14:696187e74411 873 //pc.printf("\n\rFalse Robot: X=%f,Y=%f,theta=%f", this->myX_r,this->myY_r,this->mytheta_r);
Ludwigfr 14:696187e74411 874 //pc.printf("\n\rTrue Robot: X=%f,Y=%f,theta=%f", this->myX_r_correct,this->myY_r_correct,this->mytheta_r_correct);
Ludwigfr 14:696187e74411 875 //d=this->dist(correctX, correctY, targetXWorld, targetYWorld);
Ludwigfr 14:696187e74411 876 //pc.printf("\n\r dist=%f/n", d);
Ludwigfr 14:696187e74411 877 angularLeft=200;
Ludwigfr 14:696187e74411 878 angularRight=200;
Ludwigfr 14:696187e74411 879 leftMotor(1,angularLeft);
Ludwigfr 14:696187e74411 880 rightMotor(1,angularRight);
Ludwigfr 5:19f24c363418 881
Ludwigfr 14:696187e74411 882 } while((xAtEnd-correctX)>1);
Ludwigfr 5:19f24c363418 883 //Stop at the end
Ludwigfr 5:19f24c363418 884 leftMotor(1,0);
Ludwigfr 5:19f24c363418 885 rightMotor(1,0);
Ludwigfr 14:696187e74411 886 pc.printf("\r\n REACHED");
Ludwigfr 14:696187e74411 887
Ludwigfr 14:696187e74411 888 while(1){
Ludwigfr 14:696187e74411 889 pc.printf("\n\rFalse: X=%f,Y=%f,theta=%f", this->xWorld,this->yWorld,this->thetaWorld);
Ludwigfr 14:696187e74411 890 pc.printf("\n\rTrue : X=%f,Y=%f,theta=%f", correctX,correctY,correctTheta);
Ludwigfr 14:696187e74411 891 //pc.printf("\n\rFalse Robot: X=%f,Y=%f,theta=%f", this->myX_r,this->myY_r,this->mytheta_r);
Ludwigfr 14:696187e74411 892 //pc.printf("\n\rTrue Robot: X=%f,Y=%f,theta=%f", this->myX_r_correct,this->myY_r_correct,this->mytheta_r_correct);
Ludwigfr 14:696187e74411 893 }
Ludwigfr 14:696187e74411 894 }
Ludwigfr 14:696187e74411 895
Ludwigfr 14:696187e74411 896 void MiniExplorerCoimbra::testOdometria(float *D_Special,float *L_Special){
Ludwigfr 14:696187e74411 897 float encoderRightFailureRate_Special=1;
Ludwigfr 14:696187e74411 898 float encoderLeftFailureRate_Special=0.95;
Ludwigfr 14:696187e74411 899
Ludwigfr 14:696187e74411 900 long int ticks1d_Special=R_encoder();
Ludwigfr 14:696187e74411 901 long int ticks1e_Special=L_encoder();
Ludwigfr 14:696187e74411 902
Ludwigfr 14:696187e74411 903 long int D_ticks_Special=ticks1d_Special - this->ticks2d_Special;
Ludwigfr 14:696187e74411 904 long int E_ticks_Special=ticks1e_Special - this->ticks2e_Special;
Ludwigfr 14:696187e74411 905
Ludwigfr 14:696187e74411 906 this->ticks2d_Special=ticks1d_Special;
Ludwigfr 14:696187e74411 907 this->ticks2e_Special=ticks1e_Special;
Ludwigfr 14:696187e74411 908
Ludwigfr 14:696187e74411 909 *D_Special= (float)D_ticks_Special*((3.25f*3.1415f)/4096);
Ludwigfr 14:696187e74411 910 *L_Special= (float)E_ticks_Special*((3.25f*3.1415f)/4096);
Ludwigfr 14:696187e74411 911
Ludwigfr 14:696187e74411 912 float CM_O=(*D_Special + *L_Special)/2;
Ludwigfr 14:696187e74411 913
Ludwigfr 14:696187e74411 914 //pc.printf("\r\nCM:%f",CM);
Ludwigfr 14:696187e74411 915 this->mytheta_r_correct +=(*D_Special - *L_Special)/7.18;
Ludwigfr 14:696187e74411 916
Ludwigfr 14:696187e74411 917 //modify D, L
Ludwigfr 14:696187e74411 918 float temp_Special=*D_Special;
Ludwigfr 14:696187e74411 919 *D_Special=temp_Special*encoderRightFailureRate_Special;
Ludwigfr 14:696187e74411 920 temp_Special=*L_Special;
Ludwigfr 14:696187e74411 921 *L_Special=temp_Special*encoderLeftFailureRate_Special;
Ludwigfr 14:696187e74411 922
Ludwigfr 14:696187e74411 923 float CM_Special=(*D_Special + *L_Special)/2;
Ludwigfr 14:696187e74411 924
Ludwigfr 14:696187e74411 925 this->mytheta_r +=(*D_Special -*L_Special)/7.18;
Ludwigfr 14:696187e74411 926
Ludwigfr 14:696187e74411 927 this->mytheta_r = atan2(sin(this->mytheta_r), cos(this->mytheta_r));
Ludwigfr 14:696187e74411 928 this->myX_r += CM_Special*cos(this->mytheta_r);
Ludwigfr 14:696187e74411 929 this->myY_r += CM_Special*sin(this->mytheta_r);
Ludwigfr 14:696187e74411 930
Ludwigfr 14:696187e74411 931 this->mytheta_r_correct = atan2(sin(this->mytheta_r_correct), cos(this->mytheta_r_correct));
Ludwigfr 14:696187e74411 932 this->myX_r_correct += CM_O*cos(this->mytheta_r_correct);
Ludwigfr 14:696187e74411 933 this->myY_r_correct += CM_O*sin(this->mytheta_r_correct);
Ludwigfr 5:19f24c363418 934 }
Ludwigfr 5:19f24c363418 935
Ludwigfr 10:d0109d7cbe7c 936 void MiniExplorerCoimbra::OdometriaKalmanFilter(){
Ludwigfr 14:696187e74411 937 float L_CM=0;
Ludwigfr 14:696187e74411 938 float R_CM=0;
Ludwigfr 14:696187e74411 939 int skip=0;//1==skip Kalman correction, just use KINEMATICS
Ludwigfr 10:d0109d7cbe7c 940 //=====KINEMATICS===========================
Ludwigfr 14:696187e74411 941 this->testOdometria(&R_CM,&L_CM);
Ludwigfr 14:696187e74411 942 //pc.printf("\n\rK3: D_cm:%f,L_cm:%f", (float)this->D_cm,(float)this->L_cm);
Ludwigfr 14:696187e74411 943 //pc.printf("\n\rK3: X_r=%f", X);
Ludwigfr 14:696187e74411 944 //pc.printf("\n\rK3: Y_r=%f", Y);
Ludwigfr 14:696187e74411 945 //pc.printf("\n\rK3: theta_r=%f", theta);
Ludwigfr 13:b0ddd71e8f08 946
Ludwigfr 14:696187e74411 947 float newxWorld=-this->myY_r;
Ludwigfr 14:696187e74411 948 float newyWorld=this->myX_r;
Ludwigfr 14:696187e74411 949 float newthetaWorld;
Ludwigfr 14:696187e74411 950 if(this->mytheta_r >PI/2)
Ludwigfr 14:696187e74411 951 newthetaWorld=-PI+(this->mytheta_r-PI/2);
Ludwigfr 12:1e80471c5c6c 952 else
Ludwigfr 14:696187e74411 953 newthetaWorld=this->mytheta_r+PI/2;
Ludwigfr 14:696187e74411 954
Ludwigfr 14:696187e74411 955 float distanceMoved=(R_CM+L_CM)/2;
Ludwigfr 14:696187e74411 956 float angleMovedR=(R_CM-L_CM)/this->distanceWheels;
Ludwigfr 14:696187e74411 957 float angleMoved=0;
Ludwigfr 14:696187e74411 958 if(angleMovedR >PI/2)
Ludwigfr 14:696187e74411 959 angleMoved=-PI+(angleMovedR-PI/2);
Ludwigfr 14:696187e74411 960 else
Ludwigfr 14:696187e74411 961 angleMoved=angleMovedR+PI/2;
Ludwigfr 14:696187e74411 962
Ludwigfr 14:696187e74411 963 newxWorld=this->xWorld+distanceMoved*cos(this->thetaWorld+angleMoved/2);
Ludwigfr 14:696187e74411 964 newyWorld=this->yWorld+distanceMoved*sin(this->thetaWorld+angleMoved/2);
Ludwigfr 14:696187e74411 965 newthetaWorld=this->thetaWorld+angleMoved;
Ludwigfr 14:696187e74411 966
Ludwigfr 9:1cc27f33d3e1 967 myMatrix poseKplus1K(3,1);
Ludwigfr 14:696187e74411 968 poseKplus1K.data[0][0]=newxWorld;
Ludwigfr 14:696187e74411 969 poseKplus1K.data[1][0]=newyWorld;
Ludwigfr 14:696187e74411 970 poseKplus1K.data[2][0]=newthetaWorld;
Ludwigfr 9:1cc27f33d3e1 971
Ludwigfr 14:696187e74411 972 pc.printf("\r\nR:%f,L:%f",R_CM,L_CM);
Ludwigfr 12:1e80471c5c6c 973 if(skip==1){
Ludwigfr 12:1e80471c5c6c 974 //update pose
Ludwigfr 12:1e80471c5c6c 975 this->xWorld=poseKplus1K.data[0][0];
Ludwigfr 12:1e80471c5c6c 976 this->yWorld=poseKplus1K.data[1][0];
Ludwigfr 12:1e80471c5c6c 977 this->thetaWorld=poseKplus1K.data[2][0];
Ludwigfr 14:696187e74411 978 //pc.printf("\r\nWithout Kalman X=%f, Y=%f, theta=%f",xWorld,yWorld,thetaWorld);
Ludwigfr 14:696187e74411 979 }else{
Ludwigfr 14:696187e74411 980 //note if the encoders are correct it should work
Ludwigfr 14:696187e74411 981 //pc.printf("\r\nWithout correction: X=%f, Y=%f, theta=%f",poseKplus1K.get(0,0),poseKplus1K.get(1,0),poseKplus1K.get(2,0));
Ludwigfr 14:696187e74411 982 //=====ERROR_MODEL===========================
Ludwigfr 14:696187e74411 983
Ludwigfr 14:696187e74411 984 //FP Matrix slide LocalizationKALMAN
Ludwigfr 14:696187e74411 985 myMatrix Fp(3,3);
Ludwigfr 14:696187e74411 986 Fp.data[0][0]=1;
Ludwigfr 14:696187e74411 987 Fp.data[1][1]=1;
Ludwigfr 14:696187e74411 988 Fp.data[2][2]=1;
Ludwigfr 14:696187e74411 989 Fp.data[0][2]=-1*distanceMoved*sin(this->thetaWorld+(angleMoved/2));
Ludwigfr 14:696187e74411 990 Fp.data[1][2]=distanceMoved*cos(this->thetaWorld+(angleMoved/2));
Ludwigfr 12:1e80471c5c6c 991
Ludwigfr 14:696187e74411 992 myMatrix FpTranspose(3,3);
Ludwigfr 14:696187e74411 993 FpTranspose.fillWithTranspose(Fp);
Ludwigfr 14:696187e74411 994 pc.printf("\r\nFp");;
Ludwigfr 14:696187e74411 995 //Frl matrix slide LocalizationKALMAN
Ludwigfr 14:696187e74411 996 myMatrix Frl(3,2);
Ludwigfr 14:696187e74411 997 Frl.data[0][0]=0.5*cos(this->thetaWorld+(angleMoved/2))-(distanceMoved/(2*this->distanceWheels))*sin(this->thetaWorld+(angleMoved/2));
Ludwigfr 14:696187e74411 998 Frl.data[0][1]=0.5*cos(this->thetaWorld+(angleMoved/2))+(distanceMoved/(2*this->distanceWheels))*sin(this->thetaWorld+(angleMoved/2));
Ludwigfr 14:696187e74411 999 Frl.data[1][0]=0.5*sin(this->thetaWorld+(angleMoved/2))+(distanceMoved/(2*this->distanceWheels))*cos(this->thetaWorld+(angleMoved/2));
Ludwigfr 14:696187e74411 1000 Frl.data[1][1]=0.5*sin(this->thetaWorld+(angleMoved/2))-(distanceMoved/(2*this->distanceWheels))*cos(this->thetaWorld+(angleMoved/2));
Ludwigfr 14:696187e74411 1001 Frl.data[2][0]=(1/this->distanceWheels);
Ludwigfr 14:696187e74411 1002 Frl.data[2][1]=-(1/this->distanceWheels) ;
Ludwigfr 14:696187e74411 1003
Ludwigfr 14:696187e74411 1004 myMatrix FrlTranspose(2,3);
Ludwigfr 14:696187e74411 1005 FrlTranspose.fillWithTranspose(Frl);
Ludwigfr 14:696187e74411 1006 pc.printf("\r\nFrl");
Ludwigfr 14:696187e74411 1007 //error constants...
Ludwigfr 14:696187e74411 1008 float kr=1;
Ludwigfr 14:696187e74411 1009 float kl=1;
Ludwigfr 14:696187e74411 1010 myMatrix covar(2,2);
Ludwigfr 14:696187e74411 1011 covar.data[0][0]=kr*abs(R_CM);
Ludwigfr 14:696187e74411 1012 covar.data[1][1]=kl*abs(L_CM);
Ludwigfr 14:696187e74411 1013 pc.printf("\r\ncovar");
Ludwigfr 14:696187e74411 1014 //uncertainty positionx, and theta at
Ludwigfr 14:696187e74411 1015 //1,1,1
Ludwigfr 14:696187e74411 1016 myMatrix Q(3,3);
Ludwigfr 14:696187e74411 1017 Q.data[0][0]=1;
Ludwigfr 14:696187e74411 1018 Q.data[1][1]=2;
Ludwigfr 14:696187e74411 1019 Q.data[2][2]=0.01;
Ludwigfr 14:696187e74411 1020 pc.printf("\r\nQ");
Ludwigfr 14:696187e74411 1021 //new covariance= Fp*old covariance*FpTranspose +Frl*covar*FrlTranspose +Q slide LocalizationKALMAN
Ludwigfr 14:696187e74411 1022 myMatrix covariancePositionEstimationKplus1K(3,3);
Ludwigfr 14:696187e74411 1023 myMatrix temp1(3,3);
Ludwigfr 14:696187e74411 1024 temp1.fillByMultiplication(Fp,this->covariancePositionEstimationK);//Fp*old covariance
Ludwigfr 14:696187e74411 1025
Ludwigfr 14:696187e74411 1026 myMatrix temp2(3,3);
Ludwigfr 14:696187e74411 1027 temp2.fillByMultiplication(temp1,FpTranspose);//Fp*old covariance*FpTranspose
Ludwigfr 14:696187e74411 1028 //pc.printf("\r\ncovariancePositionEstimationKplus1K 1");
Ludwigfr 14:696187e74411 1029 temp1.fillWithZeroes();
Ludwigfr 14:696187e74411 1030 pc.printf("\r\ncovariancePositionEstimationKplus1K 1.1");
Ludwigfr 14:696187e74411 1031 myMatrix temp3(3,2);
Ludwigfr 14:696187e74411 1032 temp3.fillByMultiplication(Frl,covar);//Frl*covar
Ludwigfr 14:696187e74411 1033 pc.printf("\r\ncovariancePositionEstimationKplus1K 1.2");
Ludwigfr 14:696187e74411 1034 temp1.fillByMultiplication(temp3,FrlTranspose);//Frl*covar*FrlTranspose
Ludwigfr 14:696187e74411 1035 pc.printf("\r\ncovariancePositionEstimationKplus1K 2");
Ludwigfr 14:696187e74411 1036 covariancePositionEstimationKplus1K.addition(temp2);//Fp*old covariance*FpTranspose
Ludwigfr 14:696187e74411 1037 covariancePositionEstimationKplus1K.addition(temp1);//Fp*old covariance*FpTranspose +Frl*covar*FrlTranspose
Ludwigfr 14:696187e74411 1038 covariancePositionEstimationKplus1K.addition(Q);//Fp*old covariance*FpTranspose +Frl*covar*FrlTranspose +Q
Ludwigfr 14:696187e74411 1039 pc.printf("\r\ncovariancePositionEstimationKplus1K");
Ludwigfr 12:1e80471c5c6c 1040 //=====OBSERVATION=====
Ludwigfr 12:1e80471c5c6c 1041 //get the estimated measure we should have according to our knowledge of the map and the previously computed localisation
Ludwigfr 12:1e80471c5c6c 1042
Ludwigfr 12:1e80471c5c6c 1043 float leftCmEstimated=this->sonarLeft.maxRange;
Ludwigfr 12:1e80471c5c6c 1044 float frontCmEstimated=this->sonarFront.maxRange;
Ludwigfr 12:1e80471c5c6c 1045 float rightCmEstimated=this->sonarRight.maxRange;
Ludwigfr 12:1e80471c5c6c 1046 float xWorldCell;
Ludwigfr 12:1e80471c5c6c 1047 float yWorldCell;
Ludwigfr 12:1e80471c5c6c 1048 float currDistance;
Ludwigfr 12:1e80471c5c6c 1049 float xClosestCellLeft;
Ludwigfr 12:1e80471c5c6c 1050 float yClosestCellLeft;
Ludwigfr 12:1e80471c5c6c 1051 float xClosestCellFront;
Ludwigfr 12:1e80471c5c6c 1052 float yClosestCellFront;
Ludwigfr 12:1e80471c5c6c 1053 float xClosestCellRight;
Ludwigfr 12:1e80471c5c6c 1054 float yClosestCellRight;
Ludwigfr 12:1e80471c5c6c 1055
Ludwigfr 12:1e80471c5c6c 1056 //note: sonar.isInRange already incorpore the sonar position and angle relative to the robot
Ludwigfr 12:1e80471c5c6c 1057 //note curr change to isInRange: xs-xcell and not xcell-xs
Ludwigfr 12:1e80471c5c6c 1058 //get theorical distance to sonar
Ludwigfr 12:1e80471c5c6c 1059 for(int i=0;i<this->map.nbCellWidth;i++){
Ludwigfr 12:1e80471c5c6c 1060 for(int j=0;j<this->map.nbCellHeight;j++){
Ludwigfr 12:1e80471c5c6c 1061 //check if occupied, if not discard
Ludwigfr 12:1e80471c5c6c 1062 if(this->map.get_proba_cell(i,j)>0.5){
Ludwigfr 12:1e80471c5c6c 1063 //check if in sonar range
Ludwigfr 12:1e80471c5c6c 1064 xWorldCell=this->map.cell_width_coordinate_to_world(i);
Ludwigfr 12:1e80471c5c6c 1065 yWorldCell=this->map.cell_height_coordinate_to_world(j);
Ludwigfr 12:1e80471c5c6c 1066 //check left
Ludwigfr 12:1e80471c5c6c 1067 currDistance=this->sonarLeft.isInRange(xWorldCell,yWorldCell,poseKplus1K.data[0][0],poseKplus1K.data[1][0],poseKplus1K.data[2][0]);
Ludwigfr 12:1e80471c5c6c 1068 if((currDistance < this->sonarLeft.maxRange) && currDistance > -1){
Ludwigfr 12:1e80471c5c6c 1069 //check if distance is lower than previous, update lowest if so
Ludwigfr 12:1e80471c5c6c 1070 if(currDistance < leftCmEstimated){
Ludwigfr 12:1e80471c5c6c 1071 leftCmEstimated= currDistance;
Ludwigfr 12:1e80471c5c6c 1072 xClosestCellLeft=xWorldCell;
Ludwigfr 12:1e80471c5c6c 1073 yClosestCellLeft=yWorldCell;
Ludwigfr 12:1e80471c5c6c 1074 }
Ludwigfr 12:1e80471c5c6c 1075 }
Ludwigfr 12:1e80471c5c6c 1076 //check front
Ludwigfr 12:1e80471c5c6c 1077 currDistance=this->sonarFront.isInRange(xWorldCell,yWorldCell,poseKplus1K.data[0][0],poseKplus1K.data[1][0],poseKplus1K.data[2][0]);
Ludwigfr 12:1e80471c5c6c 1078 if((currDistance < this->sonarFront.maxRange) && currDistance > -1){
Ludwigfr 12:1e80471c5c6c 1079 //check if distance is lower than previous, update lowest if so
Ludwigfr 12:1e80471c5c6c 1080 if(currDistance < frontCmEstimated){
Ludwigfr 12:1e80471c5c6c 1081 frontCmEstimated= currDistance;
Ludwigfr 12:1e80471c5c6c 1082 xClosestCellFront=xWorldCell;
Ludwigfr 12:1e80471c5c6c 1083 yClosestCellFront=yWorldCell;
Ludwigfr 12:1e80471c5c6c 1084 }
Ludwigfr 12:1e80471c5c6c 1085 }
Ludwigfr 12:1e80471c5c6c 1086 //check right
Ludwigfr 12:1e80471c5c6c 1087 currDistance=this->sonarRight.isInRange(xWorldCell,yWorldCell,poseKplus1K.data[0][0],poseKplus1K.data[1][0],poseKplus1K.data[2][0]);
Ludwigfr 12:1e80471c5c6c 1088 if((currDistance < this->sonarRight.maxRange) && currDistance > -1){
Ludwigfr 12:1e80471c5c6c 1089 //check if distance is lower than previous, update lowest if so
Ludwigfr 12:1e80471c5c6c 1090 if(currDistance < rightCmEstimated){
Ludwigfr 12:1e80471c5c6c 1091 rightCmEstimated= currDistance;
Ludwigfr 12:1e80471c5c6c 1092 xClosestCellRight=xWorldCell;
Ludwigfr 12:1e80471c5c6c 1093 yClosestCellRight=yWorldCell;
Ludwigfr 12:1e80471c5c6c 1094 }
Ludwigfr 12:1e80471c5c6c 1095 }
Ludwigfr 12:1e80471c5c6c 1096 }
Ludwigfr 12:1e80471c5c6c 1097 }
Ludwigfr 12:1e80471c5c6c 1098 }
Ludwigfr 9:1cc27f33d3e1 1099
Ludwigfr 12:1e80471c5c6c 1100 //check measurements from sonars, see if they passed the validation gate
Ludwigfr 12:1e80471c5c6c 1101 float leftCm = get_distance_left_sensor()/10;
Ludwigfr 12:1e80471c5c6c 1102 float frontCm = get_distance_front_sensor()/10;
Ludwigfr 12:1e80471c5c6c 1103 float rightCm = get_distance_right_sensor()/10;
Ludwigfr 12:1e80471c5c6c 1104
Ludwigfr 12:1e80471c5c6c 1105 //if superior to sonar range, put the value to sonar max range
Ludwigfr 12:1e80471c5c6c 1106 if(leftCm > this->sonarLeft.maxRange)
Ludwigfr 12:1e80471c5c6c 1107 leftCm=this->sonarLeft.maxRange;
Ludwigfr 12:1e80471c5c6c 1108 if(frontCm > this->sonarFront.maxRange)
Ludwigfr 12:1e80471c5c6c 1109 frontCm=this->sonarFront.maxRange;
Ludwigfr 12:1e80471c5c6c 1110 if(rightCm > this->sonarRight.maxRange)
Ludwigfr 12:1e80471c5c6c 1111 rightCm=this->sonarRight.maxRange;
Ludwigfr 12:1e80471c5c6c 1112
Ludwigfr 12:1e80471c5c6c 1113 pc.printf("\r\ndistance sonars read Lcm=%f, FCm=%f, RCm=%f",leftCm,frontCm,rightCm);
Ludwigfr 12:1e80471c5c6c 1114 pc.printf("\r\ndistance sonars estimated ELcm=%f, EFCm=%f, ERCm=%f",leftCmEstimated,frontCmEstimated,rightCmEstimated);
Ludwigfr 9:1cc27f33d3e1 1115
Ludwigfr 12:1e80471c5c6c 1116 //======INNOVATION========
Ludwigfr 12:1e80471c5c6c 1117 //compute jacobian of observation
Ludwigfr 12:1e80471c5c6c 1118 myMatrix jacobianOfObservationLeft(1,3);
Ludwigfr 12:1e80471c5c6c 1119 myMatrix jacobianOfObservationRight(1,3);
Ludwigfr 12:1e80471c5c6c 1120 myMatrix jacobianOfObservationFront(1,3);
Ludwigfr 12:1e80471c5c6c 1121
Ludwigfr 12:1e80471c5c6c 1122 //(5)
Ludwigfr 13:b0ddd71e8f08 1123 //note: for cos/sin having abs(angles) > PI is not an issue
Ludwigfr 12:1e80471c5c6c 1124 float xSonarLeft=poseKplus1K.data[0][0]+this->sonarLeft.distanceX*cos(poseKplus1K.data[0][2])-this->sonarLeft.distanceY*sin(poseKplus1K.data[0][2]);
Ludwigfr 12:1e80471c5c6c 1125 float ySonarLeft=poseKplus1K.data[1][0]+this->sonarLeft.distanceX*sin(poseKplus1K.data[0][2])-this->sonarLeft.distanceY*cos(poseKplus1K.data[0][2]);
Ludwigfr 12:1e80471c5c6c 1126 float angleSonarLeft=poseKplus1K.data[0][2]+this->sonarLeft.angleFromCenter;
Ludwigfr 13:b0ddd71e8f08 1127 if(angleSonarLeft > PI)
Ludwigfr 13:b0ddd71e8f08 1128 angleSonarLeft=angleSonarLeft-2*PI;
Ludwigfr 13:b0ddd71e8f08 1129 if(angleSonarLeft < -PI)
Ludwigfr 13:b0ddd71e8f08 1130 angleSonarLeft=angleSonarLeft+2*PI;
Ludwigfr 12:1e80471c5c6c 1131
Ludwigfr 12:1e80471c5c6c 1132 float xSonarFront=poseKplus1K.data[0][0]+this->sonarFront.distanceX*cos(poseKplus1K.data[0][2])-this->sonarFront.distanceY*sin(poseKplus1K.data[0][2]);
Ludwigfr 12:1e80471c5c6c 1133 float ySonarFront=poseKplus1K.data[1][0]+this->sonarFront.distanceX*sin(poseKplus1K.data[0][2])-this->sonarFront.distanceY*cos(poseKplus1K.data[0][2]);
Ludwigfr 12:1e80471c5c6c 1134 float angleSonarFront=poseKplus1K.data[0][2]+this->sonarFront.angleFromCenter;
Ludwigfr 13:b0ddd71e8f08 1135 if(angleSonarFront > PI)
Ludwigfr 13:b0ddd71e8f08 1136 angleSonarFront=angleSonarFront-2*PI;
Ludwigfr 13:b0ddd71e8f08 1137 if(angleSonarFront < -PI)
Ludwigfr 13:b0ddd71e8f08 1138 angleSonarFront=angleSonarFront+2*PI;
Ludwigfr 12:1e80471c5c6c 1139
Ludwigfr 12:1e80471c5c6c 1140 float xSonarRight=poseKplus1K.data[0][0]+this->sonarRight.distanceX*cos(poseKplus1K.data[0][2])-this->sonarRight.distanceY*sin(poseKplus1K.data[0][2]);
Ludwigfr 12:1e80471c5c6c 1141 float ySonarRight=poseKplus1K.data[1][0]+this->sonarRight.distanceX*sin(poseKplus1K.data[0][2])-this->sonarRight.distanceY*cos(poseKplus1K.data[0][2]);
Ludwigfr 12:1e80471c5c6c 1142 float angleSonarRight=poseKplus1K.data[0][2]+this->sonarRight.angleFromCenter;
Ludwigfr 13:b0ddd71e8f08 1143 if(angleSonarRight > PI)
Ludwigfr 13:b0ddd71e8f08 1144 angleSonarRight=angleSonarRight-2*PI;
Ludwigfr 13:b0ddd71e8f08 1145 if(angleSonarRight < -PI)
Ludwigfr 13:b0ddd71e8f08 1146 angleSonarRight=angleSonarRight+2*PI;
Ludwigfr 12:1e80471c5c6c 1147
Ludwigfr 12:1e80471c5c6c 1148 //note: last line divided by d, [0][2] all angles are theta^ teacher's advice, error in paper
Ludwigfr 12:1e80471c5c6c 1149 //left
Ludwigfr 12:1e80471c5c6c 1150 jacobianOfObservationLeft.data[0][0]=(xSonarLeft-xClosestCellLeft)/leftCmEstimated;
Ludwigfr 12:1e80471c5c6c 1151 jacobianOfObservationLeft.data[0][1]=(ySonarLeft-yClosestCellLeft)/leftCmEstimated;
Ludwigfr 12:1e80471c5c6c 1152 jacobianOfObservationLeft.data[0][2]=(1/leftCmEstimated)*(xClosestCellLeft-xSonarLeft)*(this->sonarLeft.distanceX*sin(angleSonarLeft)+this->sonarLeft.distanceY*cos(angleSonarLeft))+(yClosestCellLeft-ySonarLeft)*(-this->sonarLeft.distanceX*cos(angleSonarLeft)+this->sonarLeft.distanceY*sin(angleSonarLeft));
Ludwigfr 12:1e80471c5c6c 1153
Ludwigfr 12:1e80471c5c6c 1154 //front
Ludwigfr 12:1e80471c5c6c 1155 jacobianOfObservationFront.data[0][0]=(xSonarFront-xClosestCellFront)/frontCmEstimated;
Ludwigfr 12:1e80471c5c6c 1156 jacobianOfObservationFront.data[0][1]=(ySonarFront-yClosestCellFront)/frontCmEstimated;
Ludwigfr 12:1e80471c5c6c 1157 jacobianOfObservationFront.data[0][2]=(1/frontCmEstimated)*(xClosestCellFront-xSonarFront)*(this->sonarFront.distanceX*sin(angleSonarFront)+this->sonarFront.distanceY*cos(angleSonarFront))+(yClosestCellFront-ySonarFront)*(-this->sonarFront.distanceX*cos(angleSonarFront)+this->sonarFront.distanceY*sin(angleSonarFront));
Ludwigfr 12:1e80471c5c6c 1158
Ludwigfr 12:1e80471c5c6c 1159 //right
Ludwigfr 12:1e80471c5c6c 1160 jacobianOfObservationRight.data[0][0]=(xSonarRight-xClosestCellRight)/rightCmEstimated;
Ludwigfr 12:1e80471c5c6c 1161 jacobianOfObservationRight.data[0][1]=(ySonarRight-yClosestCellRight)/rightCmEstimated;
Ludwigfr 12:1e80471c5c6c 1162 jacobianOfObservationRight.data[0][2]=(xClosestCellRight-xSonarRight)*(xSonarRight*sin(poseKplus1K.data[2][0])+ySonarRight*cos(poseKplus1K.data[2][0]))+(yClosestCellRight-ySonarRight)*(-xSonarRight*cos(poseKplus1K.data[2][0])+ySonarRight*sin(poseKplus1K.data[2][0]));
Ludwigfr 12:1e80471c5c6c 1163 jacobianOfObservationRight.data[0][2]=(1/rightCmEstimated)*(xClosestCellRight-xSonarRight)*(this->sonarRight.distanceX*sin(angleSonarRight)+this->sonarRight.distanceY*cos(angleSonarRight))+(yClosestCellRight-ySonarRight)*(-this->sonarRight.distanceX*cos(angleSonarRight)+this->sonarRight.distanceY*sin(angleSonarRight));
Ludwigfr 9:1cc27f33d3e1 1164
Ludwigfr 12:1e80471c5c6c 1165 myMatrix jacobianOfObservationRightTranspose(3,1);
Ludwigfr 12:1e80471c5c6c 1166 jacobianOfObservationRightTranspose.fillWithTranspose(jacobianOfObservationRight);
Ludwigfr 12:1e80471c5c6c 1167
Ludwigfr 12:1e80471c5c6c 1168 myMatrix jacobianOfObservationFrontTranspose(3,1);
Ludwigfr 12:1e80471c5c6c 1169 jacobianOfObservationFrontTranspose.fillWithTranspose(jacobianOfObservationFront);
Ludwigfr 12:1e80471c5c6c 1170
Ludwigfr 12:1e80471c5c6c 1171 myMatrix jacobianOfObservationLeftTranspose(3,1);
Ludwigfr 12:1e80471c5c6c 1172 jacobianOfObservationLeftTranspose.fillWithTranspose(jacobianOfObservationLeft);
Ludwigfr 13:b0ddd71e8f08 1173 //error constants 0,0,0 sonars perfect; must be found by experimenting; gives mean and standart deviation
Ludwigfr 12:1e80471c5c6c 1174 //in centimeters
Ludwigfr 12:1e80471c5c6c 1175 float R_front=4;
Ludwigfr 12:1e80471c5c6c 1176 float R_left=4;
Ludwigfr 12:1e80471c5c6c 1177 float R_right=4;
Ludwigfr 12:1e80471c5c6c 1178
Ludwigfr 12:1e80471c5c6c 1179 //R-> 4 in diagonal
Ludwigfr 12:1e80471c5c6c 1180
Ludwigfr 13:b0ddd71e8f08 1181 //for each sonar (concatenated covariance matrix of innovation)
Ludwigfr 12:1e80471c5c6c 1182 //equ (12), innovationCovariance =JacobianObservation*covariancePositionEstimationKplus1K*JacobianObservationTranspose+R
Ludwigfr 12:1e80471c5c6c 1183 myMatrix temp4(1,3);
Ludwigfr 12:1e80471c5c6c 1184 temp4.fillByMultiplication(jacobianOfObservationFront,covariancePositionEstimationKplus1K);
Ludwigfr 12:1e80471c5c6c 1185 myMatrix temp5(1,1);
Ludwigfr 12:1e80471c5c6c 1186 temp5.fillByMultiplication(temp4,jacobianOfObservationFrontTranspose);
Ludwigfr 12:1e80471c5c6c 1187 float innovationCovarianceFront=temp5.data[0][0]+R_front;
Ludwigfr 12:1e80471c5c6c 1188 temp4.fillWithZeroes();
Ludwigfr 12:1e80471c5c6c 1189 temp5.fillWithZeroes();
Ludwigfr 12:1e80471c5c6c 1190
Ludwigfr 12:1e80471c5c6c 1191 temp4.fillByMultiplication(jacobianOfObservationLeft,covariancePositionEstimationKplus1K);
Ludwigfr 12:1e80471c5c6c 1192 temp5.fillByMultiplication(temp4,jacobianOfObservationLeftTranspose);
Ludwigfr 12:1e80471c5c6c 1193 float innovationCovarianceLeft=temp5.data[0][0]+R_left;
Ludwigfr 12:1e80471c5c6c 1194 temp4.fillWithZeroes();
Ludwigfr 12:1e80471c5c6c 1195 temp5.fillWithZeroes();
Ludwigfr 12:1e80471c5c6c 1196
Ludwigfr 12:1e80471c5c6c 1197 temp4.fillByMultiplication(jacobianOfObservationRight,covariancePositionEstimationKplus1K);
Ludwigfr 12:1e80471c5c6c 1198 temp5.fillByMultiplication(temp4,jacobianOfObservationRightTranspose);
Ludwigfr 12:1e80471c5c6c 1199 float innovationCovarianceRight=temp5.data[0][0]+R_right;
Ludwigfr 12:1e80471c5c6c 1200
Ludwigfr 12:1e80471c5c6c 1201 this->printMatrix(jacobianOfObservationLeft);
Ludwigfr 12:1e80471c5c6c 1202 this->printMatrix(jacobianOfObservationFront);
Ludwigfr 12:1e80471c5c6c 1203 this->printMatrix(jacobianOfObservationRight);
Ludwigfr 12:1e80471c5c6c 1204 pc.printf("\r\ninnovationCovariance L:%f, F:%f, R:%f",innovationCovarianceLeft,innovationCovarianceFront,innovationCovarianceRight);
Ludwigfr 12:1e80471c5c6c 1205
Ludwigfr 12:1e80471c5c6c 1206 //get the innoncation: the value of the difference between the actual measure and what we anticipated
Ludwigfr 12:1e80471c5c6c 1207 float innovationLeft=leftCm-leftCmEstimated;
Ludwigfr 12:1e80471c5c6c 1208 float innovationFront=frontCm-frontCmEstimated;
Ludwigfr 12:1e80471c5c6c 1209 float innovationRight=rightCm-rightCmEstimated;
Ludwigfr 12:1e80471c5c6c 1210
Ludwigfr 12:1e80471c5c6c 1211 //check if it pass the validation gate
Ludwigfr 12:1e80471c5c6c 1212 float gateScoreLeft=innovationLeft*innovationLeft/innovationCovarianceLeft;
Ludwigfr 12:1e80471c5c6c 1213 float gateScoreFront=innovationFront*innovationFront/innovationCovarianceFront;
Ludwigfr 12:1e80471c5c6c 1214 float gateScoreRight=innovationRight*innovationRight/innovationCovarianceRight;
Ludwigfr 12:1e80471c5c6c 1215
Ludwigfr 12:1e80471c5c6c 1216 //5cm -> 25
Ludwigfr 12:1e80471c5c6c 1217 int errorsquare=25;//constant error
Ludwigfr 12:1e80471c5c6c 1218
Ludwigfr 12:1e80471c5c6c 1219 int nbPassed=0;
Ludwigfr 12:1e80471c5c6c 1220
Ludwigfr 12:1e80471c5c6c 1221 if(gateScoreLeft <= errorsquare)
Ludwigfr 12:1e80471c5c6c 1222 nbPassed+=1;
Ludwigfr 12:1e80471c5c6c 1223 if(gateScoreFront <= errorsquare)
Ludwigfr 12:1e80471c5c6c 1224 nbPassed+=10;
Ludwigfr 12:1e80471c5c6c 1225 if(gateScoreRight <= errorsquare)
Ludwigfr 12:1e80471c5c6c 1226 nbPassed+=100;
Ludwigfr 12:1e80471c5c6c 1227 //for those who passed
Ludwigfr 12:1e80471c5c6c 1228 //compute composite innovation
Ludwigfr 12:1e80471c5c6c 1229
Ludwigfr 12:1e80471c5c6c 1230 pc.printf("\r\nscore L:%f, F:%f, R:%f",gateScoreLeft,gateScoreFront,gateScoreRight);
Ludwigfr 12:1e80471c5c6c 1231 pc.printf("\r\nnbPassed=%d",nbPassed);
Ludwigfr 9:1cc27f33d3e1 1232
Ludwigfr 12:1e80471c5c6c 1233 //compositeMeasurementJacobian
Ludwigfr 12:1e80471c5c6c 1234 myMatrix compositeMeasurementJacobian3x3(3,3);
Ludwigfr 12:1e80471c5c6c 1235 myMatrix compositeMeasurementJacobian2x3(2,3);
Ludwigfr 12:1e80471c5c6c 1236 myMatrix compositeMeasurementJacobian1x3(1,3);
Ludwigfr 12:1e80471c5c6c 1237
Ludwigfr 12:1e80471c5c6c 1238 myMatrix compositeMeasurementJacobian3x3Transpose(3,3);
Ludwigfr 12:1e80471c5c6c 1239 myMatrix compositeMeasurementJacobian2x3Transpose(3,2);
Ludwigfr 12:1e80471c5c6c 1240 myMatrix compositeMeasurementJacobian1x3Transpose(3,1);
Ludwigfr 9:1cc27f33d3e1 1241
Ludwigfr 12:1e80471c5c6c 1242 //compositeInnovation
Ludwigfr 12:1e80471c5c6c 1243 myMatrix compositeInnovation3x1(3,1);
Ludwigfr 12:1e80471c5c6c 1244 myMatrix compositeInnovation2x1(2,1);
Ludwigfr 12:1e80471c5c6c 1245 myMatrix compositeInnovation1x1(1,1);
Ludwigfr 12:1e80471c5c6c 1246
Ludwigfr 12:1e80471c5c6c 1247 //compositeInnovationCovariance
Ludwigfr 12:1e80471c5c6c 1248 myMatrix compositeInnovationCovariance3x3(3,3);
Ludwigfr 12:1e80471c5c6c 1249 myMatrix compositeInnovationCovariance2x2(2,2);
Ludwigfr 12:1e80471c5c6c 1250 myMatrix compositeInnovationCovariance1x1(1,1);
Ludwigfr 12:1e80471c5c6c 1251
Ludwigfr 12:1e80471c5c6c 1252 myMatrix tempCompositeInnovationCovariance3x3(3,3);
Ludwigfr 12:1e80471c5c6c 1253 myMatrix tempCompositeInnovationCovariance2x2(2,2);
Ludwigfr 12:1e80471c5c6c 1254
Ludwigfr 12:1e80471c5c6c 1255 myMatrix compositeInnovationCovariance3x3Inverse(3,3);
Ludwigfr 12:1e80471c5c6c 1256 myMatrix compositeInnovationCovariance2x2Inverse(2,2);
Ludwigfr 12:1e80471c5c6c 1257 myMatrix compositeInnovationCovariance1x1Inverse(1,1);
Ludwigfr 12:1e80471c5c6c 1258
Ludwigfr 12:1e80471c5c6c 1259 //Kalman Gain
Ludwigfr 12:1e80471c5c6c 1260 myMatrix kalmanGain3X3(3,3);
Ludwigfr 12:1e80471c5c6c 1261 myMatrix kalmanGain3X2(3,2);
Ludwigfr 12:1e80471c5c6c 1262 myMatrix kalmanGain3X1(3,1);
Ludwigfr 12:1e80471c5c6c 1263
Ludwigfr 12:1e80471c5c6c 1264 myMatrix tempKalmanGain3X3(3,3);
Ludwigfr 12:1e80471c5c6c 1265 myMatrix tempKalmanGain3X2(3,2);
Ludwigfr 12:1e80471c5c6c 1266 myMatrix tempKalmanGain3X1(3,1);
Ludwigfr 12:1e80471c5c6c 1267
Ludwigfr 12:1e80471c5c6c 1268 myMatrix kalmanGain3X3Transpose(3,3);
Ludwigfr 12:1e80471c5c6c 1269 myMatrix kalmanGain3X2Transpose(2,3);
Ludwigfr 12:1e80471c5c6c 1270 myMatrix kalmanGain3X1Transpose(1,3);
Ludwigfr 12:1e80471c5c6c 1271
Ludwigfr 12:1e80471c5c6c 1272 //new pose estimation
Ludwigfr 12:1e80471c5c6c 1273 myMatrix poseKplus1Kplus1(3,1);
Ludwigfr 12:1e80471c5c6c 1274 poseKplus1Kplus1.fillByCopy(poseKplus1K);
Ludwigfr 12:1e80471c5c6c 1275 myMatrix tempPoseKplus1Kplus1(3,1);
Ludwigfr 12:1e80471c5c6c 1276
Ludwigfr 12:1e80471c5c6c 1277 //covariancePositionEstimationKplus1Kplus1
Ludwigfr 12:1e80471c5c6c 1278 myMatrix covariancePositionEstimationKplus1Kplus1(3,3);
Ludwigfr 12:1e80471c5c6c 1279 covariancePositionEstimationKplus1Kplus1.fillByCopy(covariancePositionEstimationKplus1K);
Ludwigfr 12:1e80471c5c6c 1280
Ludwigfr 12:1e80471c5c6c 1281 myMatrix temp2CovariancePositionEstimationKplus1Kplus13x3(3,3);
Ludwigfr 12:1e80471c5c6c 1282 myMatrix tempCovariancePositionEstimationKplus1Kplus13x3(3,3);
Ludwigfr 12:1e80471c5c6c 1283 myMatrix tempCovariancePositionEstimationKplus1Kplus13x2(3,2);
Ludwigfr 12:1e80471c5c6c 1284 myMatrix tempCovariancePositionEstimationKplus1Kplus13x1(3,1);
Ludwigfr 12:1e80471c5c6c 1285
Ludwigfr 12:1e80471c5c6c 1286 //we do not use the composite measurement jacobian (16), we directly use the values from the measurement jacobian (jacobianOfObservation)
Ludwigfr 12:1e80471c5c6c 1287 switch(nbPassed){
Ludwigfr 12:1e80471c5c6c 1288 case 0://none
Ludwigfr 12:1e80471c5c6c 1289 //nothings happens it's okay
Ludwigfr 12:1e80471c5c6c 1290 pc.printf("\r\n000");
Ludwigfr 12:1e80471c5c6c 1291 break;
Ludwigfr 12:1e80471c5c6c 1292 case 1://left
Ludwigfr 12:1e80471c5c6c 1293 //compute compositeMeasurementJacobian
Ludwigfr 12:1e80471c5c6c 1294 //here compositeMeasurementJacobian= jacobianOfObservationLeft
Ludwigfr 12:1e80471c5c6c 1295 compositeMeasurementJacobian1x3.fillByCopy(jacobianOfObservationLeft);
Ludwigfr 12:1e80471c5c6c 1296 pc.printf("\r\n1:calcul compositeMeasurementJacobian1x3 passed");
Ludwigfr 12:1e80471c5c6c 1297 //get the compositeMeasurementJacobianTranspose
Ludwigfr 12:1e80471c5c6c 1298 compositeMeasurementJacobian1x3Transpose.fillWithTranspose(compositeMeasurementJacobian1x3);
Ludwigfr 12:1e80471c5c6c 1299 //compute compositeInnovation
Ludwigfr 12:1e80471c5c6c 1300 //here compositeInnovation=innovationLeft
Ludwigfr 12:1e80471c5c6c 1301 compositeInnovation1x1.data[0][0]=innovationLeft;
Ludwigfr 12:1e80471c5c6c 1302 pc.printf("\r\n1:calcul compositeInnovation1x1 passed");
Ludwigfr 12:1e80471c5c6c 1303 //compute compositeInnovationCovariance=compositeMeasurementJacobian*covariancePositionEstimationKplus1K*compositeMeasurementJacobianTranpose
Ludwigfr 12:1e80471c5c6c 1304 //add the right R on the diagonal
Ludwigfr 12:1e80471c5c6c 1305 //here compositeInnovationCovariance=innovationCovarianceLeft
Ludwigfr 12:1e80471c5c6c 1306 compositeInnovationCovariance1x1.data[0][0]=innovationCovarianceLeft;
Ludwigfr 12:1e80471c5c6c 1307 pc.printf("\r\n1:calcul compositeInnovationCovariance1x1 passed");
Ludwigfr 12:1e80471c5c6c 1308 //get the inverse of the compositeInnovationCovariance
Ludwigfr 12:1e80471c5c6c 1309 compositeInnovationCovariance1x1Inverse.data[0][0]=1/innovationCovarianceLeft;
Ludwigfr 12:1e80471c5c6c 1310 pc.printf("\r\n1:calcul compositeInnovationCovariance1x1Inverse passed");
Ludwigfr 12:1e80471c5c6c 1311 //compute KalmanGain=covariancePositionEstimationKplus1K*compositeMeasurementJacobianTranspose*Inverse(compositeInnovationCovariance)
Ludwigfr 12:1e80471c5c6c 1312 tempKalmanGain3X1.fillByMultiplication(covariancePositionEstimationKplus1K,compositeMeasurementJacobian1x3Transpose);
Ludwigfr 12:1e80471c5c6c 1313 kalmanGain3X1.fillByMultiplication(tempKalmanGain3X1,compositeInnovationCovariance1x1Inverse);
Ludwigfr 12:1e80471c5c6c 1314 pc.printf("\r\n1:calcul kalmanGain3X1 passed");
Ludwigfr 12:1e80471c5c6c 1315 //get the transpose of the kalman gain
Ludwigfr 12:1e80471c5c6c 1316 kalmanGain3X1Transpose.fillWithTranspose(kalmanGain3X1);
Ludwigfr 12:1e80471c5c6c 1317 //update pose estimation=old pose estimation + kalman gain*compositeInnovation
Ludwigfr 12:1e80471c5c6c 1318 tempPoseKplus1Kplus1.fillByMultiplication(kalmanGain3X1,compositeInnovation1x1);
Ludwigfr 12:1e80471c5c6c 1319 poseKplus1Kplus1.addition(tempPoseKplus1Kplus1);
Ludwigfr 12:1e80471c5c6c 1320 pc.printf("\r\n1:calcul poseKplus1Kplus1 passed");
Ludwigfr 12:1e80471c5c6c 1321 //compute covariancePositionEstimationKplus1Kplus1=covariancePositionEstimationKplus1K-kalmanGain*compositeInnovationCovariance*KalmanGainTranspose
Ludwigfr 12:1e80471c5c6c 1322 tempCovariancePositionEstimationKplus1Kplus13x1.fillByMultiplication(kalmanGain3X1,compositeInnovationCovariance1x1);
Ludwigfr 12:1e80471c5c6c 1323 tempCovariancePositionEstimationKplus1Kplus13x3.fillByMultiplication(tempCovariancePositionEstimationKplus1Kplus13x1,kalmanGain3X1Transpose);
Ludwigfr 12:1e80471c5c6c 1324 covariancePositionEstimationKplus1Kplus1.subtraction(tempCovariancePositionEstimationKplus1Kplus13x3);
Ludwigfr 12:1e80471c5c6c 1325 pc.printf("\r\n1:calcul covariancePositionEstimationKplus1Kplus1 passed");
Ludwigfr 12:1e80471c5c6c 1326 break;
Ludwigfr 12:1e80471c5c6c 1327 case 10://front
Ludwigfr 12:1e80471c5c6c 1328 //compute compositeMeasurementJacobian
Ludwigfr 12:1e80471c5c6c 1329 compositeMeasurementJacobian1x3.fillByCopy(jacobianOfObservationFront);
Ludwigfr 12:1e80471c5c6c 1330 pc.printf("\r\n10:calcul compositeMeasurementJacobian1x3 passed");
Ludwigfr 12:1e80471c5c6c 1331 //get the compositeMeasurementJacobianTranspose
Ludwigfr 12:1e80471c5c6c 1332 compositeMeasurementJacobian1x3Transpose.fillWithTranspose(compositeMeasurementJacobian1x3);
Ludwigfr 12:1e80471c5c6c 1333 //compute compositeInnovation
Ludwigfr 12:1e80471c5c6c 1334 compositeInnovation1x1.data[0][0]=innovationFront;
Ludwigfr 12:1e80471c5c6c 1335 pc.printf("\r\n10:calcul compositeInnovation1x1 passed");
Ludwigfr 12:1e80471c5c6c 1336 //compute compositeInnovationCovariance=compositeMeasurementJacobian*covariancePositionEstimationKplus1K*compositeMeasurementJacobianTranpose
Ludwigfr 12:1e80471c5c6c 1337 //add the right R on the diagonal
Ludwigfr 12:1e80471c5c6c 1338 compositeInnovationCovariance1x1.data[0][0]=innovationCovarianceFront;
Ludwigfr 12:1e80471c5c6c 1339 pc.printf("\r\n10:calcul compositeInnovationCovariance1x1 passed");
Ludwigfr 12:1e80471c5c6c 1340 //get the inverse of the compositeInnovationCovariance
Ludwigfr 12:1e80471c5c6c 1341 compositeInnovationCovariance1x1Inverse.data[0][0]=1/innovationCovarianceFront;
Ludwigfr 12:1e80471c5c6c 1342 pc.printf("\r\n10:calcul compositeInnovationCovariance1x1Inverse passed");
Ludwigfr 12:1e80471c5c6c 1343 //compute KalmanGain=covariancePositionEstimationKplus1K*compositeMeasurementJacobianTranspose*Inverse(compositeInnovationCovariance)
Ludwigfr 12:1e80471c5c6c 1344 tempKalmanGain3X1.fillByMultiplication(covariancePositionEstimationKplus1K,compositeMeasurementJacobian1x3Transpose);
Ludwigfr 12:1e80471c5c6c 1345 kalmanGain3X1.fillByMultiplication(tempKalmanGain3X1,compositeInnovationCovariance1x1Inverse);
Ludwigfr 12:1e80471c5c6c 1346 pc.printf("\r\n10:calcul kalmanGain3X1 passed");
Ludwigfr 12:1e80471c5c6c 1347 //get the transpose of the kalman gain
Ludwigfr 12:1e80471c5c6c 1348 kalmanGain3X1Transpose.fillWithTranspose(kalmanGain3X1);
Ludwigfr 12:1e80471c5c6c 1349 //update pose estimation=old pose estimation + kalman gain*compositeInnovation
Ludwigfr 12:1e80471c5c6c 1350 tempPoseKplus1Kplus1.fillByMultiplication(kalmanGain3X1,compositeInnovation1x1);
Ludwigfr 12:1e80471c5c6c 1351 poseKplus1Kplus1.addition(tempPoseKplus1Kplus1);
Ludwigfr 12:1e80471c5c6c 1352 pc.printf("\r\n10:calcul poseKplus1Kplus1 passed");
Ludwigfr 12:1e80471c5c6c 1353 //compute covariancePositionEstimationKplus1Kplus1=covariancePositionEstimationKplus1K-kalmanGain*compositeInnovationCovariance*KalmanGainTranspose
Ludwigfr 12:1e80471c5c6c 1354 tempCovariancePositionEstimationKplus1Kplus13x1.fillByMultiplication(kalmanGain3X1,compositeInnovationCovariance1x1);
Ludwigfr 12:1e80471c5c6c 1355 tempCovariancePositionEstimationKplus1Kplus13x3.fillByMultiplication(tempCovariancePositionEstimationKplus1Kplus13x1,kalmanGain3X1Transpose);
Ludwigfr 12:1e80471c5c6c 1356 covariancePositionEstimationKplus1Kplus1.subtraction(tempCovariancePositionEstimationKplus1Kplus13x3);
Ludwigfr 12:1e80471c5c6c 1357 pc.printf("\r\n10:calcul covariancePositionEstimationKplus1Kplus1 passed");
Ludwigfr 12:1e80471c5c6c 1358 break;
Ludwigfr 12:1e80471c5c6c 1359 case 11://left and front
Ludwigfr 12:1e80471c5c6c 1360 //compute compositeMeasurementJacobian
Ludwigfr 12:1e80471c5c6c 1361 compositeMeasurementJacobian2x3.data[0][0]=jacobianOfObservationLeft.data[0][0];
Ludwigfr 12:1e80471c5c6c 1362 compositeMeasurementJacobian2x3.data[0][1]=jacobianOfObservationLeft.data[0][1];
Ludwigfr 12:1e80471c5c6c 1363 compositeMeasurementJacobian2x3.data[0][2]=jacobianOfObservationLeft.data[0][2];
Ludwigfr 12:1e80471c5c6c 1364
Ludwigfr 12:1e80471c5c6c 1365 compositeMeasurementJacobian2x3.data[1][0]=jacobianOfObservationFront.data[0][0];
Ludwigfr 12:1e80471c5c6c 1366 compositeMeasurementJacobian2x3.data[1][1]=jacobianOfObservationFront.data[0][1];
Ludwigfr 12:1e80471c5c6c 1367 compositeMeasurementJacobian2x3.data[1][2]=jacobianOfObservationFront.data[0][2];
Ludwigfr 12:1e80471c5c6c 1368 pc.printf("\r\n11:calcul compositeMeasurementJacobian2x3 passed");
Ludwigfr 12:1e80471c5c6c 1369 //get the compositeMeasurementJacobianTranspose
Ludwigfr 12:1e80471c5c6c 1370 compositeMeasurementJacobian2x3Transpose.fillWithTranspose(compositeMeasurementJacobian2x3);
Ludwigfr 12:1e80471c5c6c 1371 //compute compositeInnovation
Ludwigfr 12:1e80471c5c6c 1372 compositeInnovation2x1.data[0][0]=innovationLeft;
Ludwigfr 12:1e80471c5c6c 1373 compositeInnovation2x1.data[1][0]=innovationFront;
Ludwigfr 12:1e80471c5c6c 1374 pc.printf("\r\n11:calcul compositeInnovation2x1 passed");
Ludwigfr 12:1e80471c5c6c 1375 //compute compositeInnovationCovariance=compositeMeasurementJacobian*covariancePositionEstimationKplus1K*compositeMeasurementJacobianTranpose+R
Ludwigfr 12:1e80471c5c6c 1376 tempCompositeInnovationCovariance2x2.fillByMultiplication(compositeMeasurementJacobian2x3,covariancePositionEstimationKplus1K);
Ludwigfr 12:1e80471c5c6c 1377 compositeInnovationCovariance2x2.fillByMultiplication(tempCompositeInnovationCovariance2x2,compositeMeasurementJacobian2x3Transpose);
Ludwigfr 12:1e80471c5c6c 1378 //add the right R on the diagonal
Ludwigfr 12:1e80471c5c6c 1379 compositeInnovationCovariance2x2.data[0][0]+=R_left;
Ludwigfr 12:1e80471c5c6c 1380 compositeInnovationCovariance2x2.data[1][1]+=R_front;
Ludwigfr 12:1e80471c5c6c 1381 pc.printf("\r\n11:calcul compositeInnovationCovariance2x2 passed");
Ludwigfr 12:1e80471c5c6c 1382 //get the inverse of the compositeInnovationCovariance
Ludwigfr 12:1e80471c5c6c 1383 compositeInnovationCovariance2x2Inverse.fillWithInverse(compositeInnovationCovariance2x2);
Ludwigfr 12:1e80471c5c6c 1384 pc.printf("\r\n11:calcul compositeInnovationCovariance2x2Inverse passed");
Ludwigfr 12:1e80471c5c6c 1385 //compute KalmanGain=covariancePositionEstimationKplus1K*compositeMeasurementJacobianTranspose*Inverse(compositeInnovationCovariance)
Ludwigfr 12:1e80471c5c6c 1386 tempKalmanGain3X2.fillByMultiplication(covariancePositionEstimationKplus1K,compositeMeasurementJacobian2x3Transpose);
Ludwigfr 12:1e80471c5c6c 1387 kalmanGain3X2.fillByMultiplication(tempKalmanGain3X2,compositeInnovationCovariance2x2Inverse);
Ludwigfr 12:1e80471c5c6c 1388 pc.printf("\r\n11:calcul kalmanGain3X2 passed");
Ludwigfr 12:1e80471c5c6c 1389 //get the transpose of the kalman gain
Ludwigfr 12:1e80471c5c6c 1390 kalmanGain3X2Transpose.fillWithTranspose(kalmanGain3X2);
Ludwigfr 12:1e80471c5c6c 1391 //update pose estimation=old pose estimation + kalman gain*compositeInnovation
Ludwigfr 12:1e80471c5c6c 1392 tempPoseKplus1Kplus1.fillByMultiplication(kalmanGain3X2,compositeInnovation2x1);
Ludwigfr 12:1e80471c5c6c 1393 poseKplus1Kplus1.addition(tempPoseKplus1Kplus1);
Ludwigfr 12:1e80471c5c6c 1394 pc.printf("\r\n11:calcul poseKplus1Kplus1 passed");
Ludwigfr 12:1e80471c5c6c 1395 //compute covariancePositionEstimationKplus1Kplus1=covariancePositionEstimationKplus1K-kalmanGain*compositeInnovationCovariance*KalmanGainTranspose
Ludwigfr 12:1e80471c5c6c 1396 tempCovariancePositionEstimationKplus1Kplus13x2.fillByMultiplication(kalmanGain3X2,compositeInnovationCovariance2x2);
Ludwigfr 12:1e80471c5c6c 1397 tempCovariancePositionEstimationKplus1Kplus13x3.fillByMultiplication(tempCovariancePositionEstimationKplus1Kplus13x2,kalmanGain3X2Transpose);
Ludwigfr 12:1e80471c5c6c 1398 covariancePositionEstimationKplus1Kplus1.subtraction(tempCovariancePositionEstimationKplus1Kplus13x3);
Ludwigfr 12:1e80471c5c6c 1399 pc.printf("\r\n11:calcul covariancePositionEstimationKplus1Kplus1 passed");
Ludwigfr 12:1e80471c5c6c 1400 break;
Ludwigfr 12:1e80471c5c6c 1401
Ludwigfr 12:1e80471c5c6c 1402 case 100://right
Ludwigfr 12:1e80471c5c6c 1403 //compute compositeMeasurementJacobian
Ludwigfr 12:1e80471c5c6c 1404 compositeMeasurementJacobian1x3.fillByCopy(jacobianOfObservationRight);
Ludwigfr 12:1e80471c5c6c 1405 pc.printf("\r\n100:calcul compositeMeasurementJacobian1x3 passed");
Ludwigfr 12:1e80471c5c6c 1406 //get the compositeMeasurementJacobianTranspose
Ludwigfr 12:1e80471c5c6c 1407 compositeMeasurementJacobian1x3Transpose.fillWithTranspose(compositeMeasurementJacobian1x3);
Ludwigfr 12:1e80471c5c6c 1408 //compute compositeInnovation
Ludwigfr 12:1e80471c5c6c 1409 compositeInnovation1x1.data[0][0]=innovationRight;
Ludwigfr 12:1e80471c5c6c 1410 //compute compositeInnovationCovariance=compositeMeasurementJacobian*covariancePositionEstimationKplus1K*compositeMeasurementJacobianTranpose
Ludwigfr 12:1e80471c5c6c 1411 //add the right R on the diagonal
Ludwigfr 12:1e80471c5c6c 1412 compositeInnovationCovariance1x1.data[0][0]=innovationCovarianceRight;
Ludwigfr 12:1e80471c5c6c 1413 //get the inverse of the compositeInnovationCovariance
Ludwigfr 12:1e80471c5c6c 1414 compositeInnovationCovariance1x1Inverse.data[0][0]=1/innovationCovarianceRight;
Ludwigfr 12:1e80471c5c6c 1415 pc.printf("\r\n100:calcul compositeInnovationCovariance1x1Inverse passed");
Ludwigfr 12:1e80471c5c6c 1416 //compute KalmanGain=covariancePositionEstimationKplus1K*compositeMeasurementJacobianTranspose*Inverse(compositeInnovationCovariance)
Ludwigfr 12:1e80471c5c6c 1417 tempKalmanGain3X1.fillByMultiplication(covariancePositionEstimationKplus1K,compositeMeasurementJacobian1x3Transpose);
Ludwigfr 12:1e80471c5c6c 1418 kalmanGain3X1.fillByMultiplication(tempKalmanGain3X1,compositeInnovationCovariance1x1Inverse);
Ludwigfr 12:1e80471c5c6c 1419 pc.printf("\r\n100:calcul kalmanGain3X1 passed");
Ludwigfr 12:1e80471c5c6c 1420 //get the transpose of the kalman gain
Ludwigfr 12:1e80471c5c6c 1421 kalmanGain3X1Transpose.fillWithTranspose(kalmanGain3X1);
Ludwigfr 12:1e80471c5c6c 1422 //update pose estimation=old pose estimation + kalman gain*compositeInnovation
Ludwigfr 12:1e80471c5c6c 1423 tempPoseKplus1Kplus1.fillByMultiplication(kalmanGain3X1,compositeInnovation1x1);
Ludwigfr 12:1e80471c5c6c 1424 poseKplus1Kplus1.addition(tempPoseKplus1Kplus1);
Ludwigfr 12:1e80471c5c6c 1425 pc.printf("\r\n100:calcul poseKplus1Kplus1 passed");
Ludwigfr 12:1e80471c5c6c 1426 //compute covariancePositionEstimationKplus1Kplus1=covariancePositionEstimationKplus1K-kalmanGain*compositeInnovationCovariance*KalmanGainTranspose
Ludwigfr 12:1e80471c5c6c 1427 tempCovariancePositionEstimationKplus1Kplus13x1.fillByMultiplication(kalmanGain3X1,compositeInnovationCovariance1x1);
Ludwigfr 12:1e80471c5c6c 1428 tempCovariancePositionEstimationKplus1Kplus13x3.fillByMultiplication(tempCovariancePositionEstimationKplus1Kplus13x1,kalmanGain3X1Transpose);
Ludwigfr 12:1e80471c5c6c 1429 covariancePositionEstimationKplus1Kplus1.subtraction(tempCovariancePositionEstimationKplus1Kplus13x3);
Ludwigfr 12:1e80471c5c6c 1430 pc.printf("\r\n100:calcul covariancePositionEstimationKplus1Kplus1 passed");
Ludwigfr 12:1e80471c5c6c 1431 break;
Ludwigfr 12:1e80471c5c6c 1432 case 101://left and right
Ludwigfr 12:1e80471c5c6c 1433 //compute compositeMeasurementJacobian
Ludwigfr 12:1e80471c5c6c 1434 compositeMeasurementJacobian2x3.data[0][0]=jacobianOfObservationLeft.data[0][0];
Ludwigfr 12:1e80471c5c6c 1435 compositeMeasurementJacobian2x3.data[0][1]=jacobianOfObservationLeft.data[0][1];
Ludwigfr 12:1e80471c5c6c 1436 compositeMeasurementJacobian2x3.data[0][2]=jacobianOfObservationLeft.data[0][2];
Ludwigfr 12:1e80471c5c6c 1437
Ludwigfr 12:1e80471c5c6c 1438 compositeMeasurementJacobian2x3.data[1][0]=jacobianOfObservationRight.data[0][0];
Ludwigfr 12:1e80471c5c6c 1439 compositeMeasurementJacobian2x3.data[1][1]=jacobianOfObservationRight.data[0][1];
Ludwigfr 12:1e80471c5c6c 1440 compositeMeasurementJacobian2x3.data[1][2]=jacobianOfObservationRight.data[0][2];
Ludwigfr 12:1e80471c5c6c 1441 pc.printf("\r\n101:calcul compositeMeasurementJacobian2x3 passed");
Ludwigfr 12:1e80471c5c6c 1442 //get the compositeMeasurementJacobianTranspose
Ludwigfr 12:1e80471c5c6c 1443 compositeMeasurementJacobian2x3Transpose.fillWithTranspose(compositeMeasurementJacobian2x3);
Ludwigfr 12:1e80471c5c6c 1444 //compute compositeInnovation
Ludwigfr 12:1e80471c5c6c 1445 compositeInnovation2x1.data[0][0]=innovationLeft;
Ludwigfr 12:1e80471c5c6c 1446 compositeInnovation2x1.data[1][0]=innovationRight;
Ludwigfr 12:1e80471c5c6c 1447 //compute compositeInnovationCovariance=compositeMeasurementJacobian*covariancePositionEstimationKplus1K*compositeMeasurementJacobianTranpose+R
Ludwigfr 12:1e80471c5c6c 1448 tempCompositeInnovationCovariance2x2.fillByMultiplication(compositeMeasurementJacobian2x3,covariancePositionEstimationKplus1K);
Ludwigfr 12:1e80471c5c6c 1449 compositeInnovationCovariance2x2.fillByMultiplication(tempCompositeInnovationCovariance2x2,compositeMeasurementJacobian2x3Transpose);
Ludwigfr 12:1e80471c5c6c 1450 //add the right R on the diagonal
Ludwigfr 12:1e80471c5c6c 1451 compositeInnovationCovariance2x2.data[0][0]+=R_left;
Ludwigfr 12:1e80471c5c6c 1452 compositeInnovationCovariance2x2.data[1][1]+=R_right;
Ludwigfr 12:1e80471c5c6c 1453 pc.printf("\r\n101:calcul compositeInnovationCovariance2x2 passed");
Ludwigfr 12:1e80471c5c6c 1454 //get the inverse of the compositeInnovationCovariance
Ludwigfr 12:1e80471c5c6c 1455 compositeInnovationCovariance2x2Inverse.fillWithInverse(compositeInnovationCovariance2x2);
Ludwigfr 12:1e80471c5c6c 1456 pc.printf("\r\n101:calcul compositeInnovationCovariance2x2Inverse passed");
Ludwigfr 12:1e80471c5c6c 1457 //compute KalmanGain=covariancePositionEstimationKplus1K*compositeMeasurementJacobianTranspose*Inverse(compositeInnovationCovariance)
Ludwigfr 12:1e80471c5c6c 1458 tempKalmanGain3X2.fillByMultiplication(covariancePositionEstimationKplus1K,compositeMeasurementJacobian2x3Transpose);
Ludwigfr 12:1e80471c5c6c 1459 kalmanGain3X2.fillByMultiplication(tempKalmanGain3X2,compositeInnovationCovariance2x2Inverse);
Ludwigfr 12:1e80471c5c6c 1460 pc.printf("\r\n101:calcul kalmanGain3X2 passed");
Ludwigfr 12:1e80471c5c6c 1461 //get the transpose of the kalman gain
Ludwigfr 12:1e80471c5c6c 1462 kalmanGain3X2Transpose.fillWithTranspose(kalmanGain3X2);
Ludwigfr 12:1e80471c5c6c 1463 //update pose estimation=old pose estimation + kalman gain*compositeInnovation
Ludwigfr 12:1e80471c5c6c 1464 tempPoseKplus1Kplus1.fillByMultiplication(kalmanGain3X2,compositeInnovation2x1);
Ludwigfr 12:1e80471c5c6c 1465 poseKplus1Kplus1.addition(tempPoseKplus1Kplus1);
Ludwigfr 12:1e80471c5c6c 1466 pc.printf("\r\n101:calcul poseKplus1Kplus1 passed");
Ludwigfr 12:1e80471c5c6c 1467 //compute covariancePositionEstimationKplus1Kplus1=covariancePositionEstimationKplus1K-kalmanGain*compositeInnovationCovariance*KalmanGainTranspose
Ludwigfr 12:1e80471c5c6c 1468 tempCovariancePositionEstimationKplus1Kplus13x2.fillByMultiplication(kalmanGain3X2,compositeInnovationCovariance2x2);
Ludwigfr 12:1e80471c5c6c 1469 tempCovariancePositionEstimationKplus1Kplus13x3.fillByMultiplication(tempCovariancePositionEstimationKplus1Kplus13x2,kalmanGain3X2Transpose);
Ludwigfr 12:1e80471c5c6c 1470 covariancePositionEstimationKplus1Kplus1.subtraction(tempCovariancePositionEstimationKplus1Kplus13x3);
Ludwigfr 12:1e80471c5c6c 1471 pc.printf("\r\n101:calcul covariancePositionEstimationKplus1Kplus1 passed");
Ludwigfr 12:1e80471c5c6c 1472 break;
Ludwigfr 12:1e80471c5c6c 1473 case 110:// front and right
Ludwigfr 12:1e80471c5c6c 1474 //compute compositeMeasurementJacobian
Ludwigfr 12:1e80471c5c6c 1475 compositeMeasurementJacobian2x3.data[0][0]=jacobianOfObservationFront.data[0][0];
Ludwigfr 12:1e80471c5c6c 1476 compositeMeasurementJacobian2x3.data[0][1]=jacobianOfObservationFront.data[0][1];
Ludwigfr 12:1e80471c5c6c 1477 compositeMeasurementJacobian2x3.data[0][2]=jacobianOfObservationFront.data[0][2];
Ludwigfr 12:1e80471c5c6c 1478
Ludwigfr 12:1e80471c5c6c 1479 compositeMeasurementJacobian2x3.data[1][0]=jacobianOfObservationRight.data[0][0];
Ludwigfr 12:1e80471c5c6c 1480 compositeMeasurementJacobian2x3.data[1][1]=jacobianOfObservationRight.data[0][1];
Ludwigfr 12:1e80471c5c6c 1481 compositeMeasurementJacobian2x3.data[1][2]=jacobianOfObservationRight.data[0][2];
Ludwigfr 12:1e80471c5c6c 1482
Ludwigfr 12:1e80471c5c6c 1483 pc.printf("\r\n110:calcul compositeMeasurementJacobian2x3 passed");
Ludwigfr 12:1e80471c5c6c 1484 //get the compositeMeasurementJacobianTranspose
Ludwigfr 12:1e80471c5c6c 1485 compositeMeasurementJacobian2x3Transpose.fillWithTranspose(compositeMeasurementJacobian2x3);
Ludwigfr 12:1e80471c5c6c 1486 //compute compositeInnovation
Ludwigfr 12:1e80471c5c6c 1487 compositeInnovation2x1.data[0][0]=innovationFront;
Ludwigfr 12:1e80471c5c6c 1488 compositeInnovation2x1.data[1][0]=innovationRight;
Ludwigfr 12:1e80471c5c6c 1489 //compute compositeInnovationCovariance=compositeMeasurementJacobian*covariancePositionEstimationKplus1K*compositeMeasurementJacobianTranpose+R
Ludwigfr 12:1e80471c5c6c 1490 tempCompositeInnovationCovariance2x2.fillByMultiplication(compositeMeasurementJacobian2x3,covariancePositionEstimationKplus1K);
Ludwigfr 12:1e80471c5c6c 1491 compositeInnovationCovariance2x2.fillByMultiplication(tempCompositeInnovationCovariance2x2,compositeMeasurementJacobian2x3Transpose);
Ludwigfr 12:1e80471c5c6c 1492 //add the right R on the diagonal
Ludwigfr 12:1e80471c5c6c 1493 compositeInnovationCovariance2x2.data[0][0]+=R_front;
Ludwigfr 12:1e80471c5c6c 1494 compositeInnovationCovariance2x2.data[1][1]+=R_right;
Ludwigfr 12:1e80471c5c6c 1495 pc.printf("\r\n110:calcul compositeInnovationCovariance2x2 passed");
Ludwigfr 12:1e80471c5c6c 1496 //get the inverse of the compositeInnovationCovariance
Ludwigfr 12:1e80471c5c6c 1497 compositeInnovationCovariance2x2Inverse.fillWithInverse(compositeInnovationCovariance2x2);
Ludwigfr 12:1e80471c5c6c 1498 pc.printf("\r\n110:calcul compositeInnovationCovariance2x2Inverse passed");
Ludwigfr 12:1e80471c5c6c 1499 //compute KalmanGain=covariancePositionEstimationKplus1K*compositeMeasurementJacobianTranspose*Inverse(compositeInnovationCovariance)
Ludwigfr 12:1e80471c5c6c 1500 tempKalmanGain3X2.fillByMultiplication(covariancePositionEstimationKplus1K,compositeMeasurementJacobian2x3Transpose);
Ludwigfr 12:1e80471c5c6c 1501 kalmanGain3X2.fillByMultiplication(tempKalmanGain3X2,compositeInnovationCovariance2x2Inverse);
Ludwigfr 12:1e80471c5c6c 1502 pc.printf("\r\n110:calcul kalmanGain3X2 passed");
Ludwigfr 12:1e80471c5c6c 1503 //get the transpose of the kalman gain
Ludwigfr 12:1e80471c5c6c 1504 kalmanGain3X2Transpose.fillWithTranspose(kalmanGain3X2);
Ludwigfr 12:1e80471c5c6c 1505 //update pose estimation=old pose estimation + kalman gain*compositeInnovation
Ludwigfr 12:1e80471c5c6c 1506 tempPoseKplus1Kplus1.fillByMultiplication(kalmanGain3X2,compositeInnovation2x1);
Ludwigfr 12:1e80471c5c6c 1507 poseKplus1Kplus1.addition(tempPoseKplus1Kplus1);
Ludwigfr 12:1e80471c5c6c 1508 pc.printf("\r\n110:calcul poseKplus1Kplus1 passed");
Ludwigfr 12:1e80471c5c6c 1509 //compute covariancePositionEstimationKplus1Kplus1=covariancePositionEstimationKplus1K-kalmanGain*compositeInnovationCovariance*KalmanGainTranspose
Ludwigfr 12:1e80471c5c6c 1510 tempCovariancePositionEstimationKplus1Kplus13x2.fillByMultiplication(kalmanGain3X2,compositeInnovationCovariance2x2);
Ludwigfr 12:1e80471c5c6c 1511 tempCovariancePositionEstimationKplus1Kplus13x3.fillByMultiplication(tempCovariancePositionEstimationKplus1Kplus13x2,kalmanGain3X2Transpose);
Ludwigfr 12:1e80471c5c6c 1512 covariancePositionEstimationKplus1Kplus1.subtraction(tempCovariancePositionEstimationKplus1Kplus13x3);
Ludwigfr 12:1e80471c5c6c 1513 pc.printf("\r\n110:calcul covariancePositionEstimationKplus1Kplus1 passed");
Ludwigfr 12:1e80471c5c6c 1514 break;
Ludwigfr 12:1e80471c5c6c 1515 case 111://left front and right
Ludwigfr 12:1e80471c5c6c 1516 //compute compositeMeasurementJacobian
Ludwigfr 12:1e80471c5c6c 1517 compositeMeasurementJacobian3x3.data[0][0]=jacobianOfObservationLeft.data[0][0];
Ludwigfr 12:1e80471c5c6c 1518 compositeMeasurementJacobian3x3.data[0][1]=jacobianOfObservationLeft.data[0][1];
Ludwigfr 12:1e80471c5c6c 1519 compositeMeasurementJacobian3x3.data[0][2]=jacobianOfObservationLeft.data[0][2];
Ludwigfr 12:1e80471c5c6c 1520
Ludwigfr 12:1e80471c5c6c 1521 compositeMeasurementJacobian3x3.data[1][0]=jacobianOfObservationFront.data[0][0];
Ludwigfr 12:1e80471c5c6c 1522 compositeMeasurementJacobian3x3.data[1][1]=jacobianOfObservationFront.data[0][1];
Ludwigfr 12:1e80471c5c6c 1523 compositeMeasurementJacobian3x3.data[1][2]=jacobianOfObservationFront.data[0][2];
Ludwigfr 12:1e80471c5c6c 1524
Ludwigfr 12:1e80471c5c6c 1525 compositeMeasurementJacobian3x3.data[2][0]=jacobianOfObservationRight.data[0][0];
Ludwigfr 12:1e80471c5c6c 1526 compositeMeasurementJacobian3x3.data[2][1]=jacobianOfObservationRight.data[0][1];
Ludwigfr 12:1e80471c5c6c 1527 compositeMeasurementJacobian3x3.data[2][2]=jacobianOfObservationRight.data[0][2];
Ludwigfr 12:1e80471c5c6c 1528
Ludwigfr 12:1e80471c5c6c 1529 pc.printf("\r\n111:calcul compositeMeasurementJacobian3x3 passed");
Ludwigfr 12:1e80471c5c6c 1530 //get the compositeMeasurementJacobianTranspose
Ludwigfr 12:1e80471c5c6c 1531 compositeMeasurementJacobian3x3Transpose.fillWithTranspose(compositeMeasurementJacobian3x3);
Ludwigfr 12:1e80471c5c6c 1532 //compute compositeInnovation
Ludwigfr 12:1e80471c5c6c 1533 compositeInnovation3x1.data[0][0]=innovationLeft;
Ludwigfr 12:1e80471c5c6c 1534 compositeInnovation3x1.data[1][0]=innovationFront;
Ludwigfr 12:1e80471c5c6c 1535 compositeInnovation3x1.data[2][0]=innovationRight;
Ludwigfr 12:1e80471c5c6c 1536 //compute compositeInnovationCovariance=compositeMeasurementJacobian*covariancePositionEstimationKplus1K*compositeMeasurementJacobianTranpose+R
Ludwigfr 12:1e80471c5c6c 1537 tempCompositeInnovationCovariance3x3.fillByMultiplication(compositeMeasurementJacobian3x3,covariancePositionEstimationKplus1K);
Ludwigfr 12:1e80471c5c6c 1538 compositeInnovationCovariance3x3.fillByMultiplication(tempCompositeInnovationCovariance3x3,compositeMeasurementJacobian3x3Transpose);
Ludwigfr 12:1e80471c5c6c 1539 //add the right R on the diagonal
Ludwigfr 12:1e80471c5c6c 1540 compositeInnovationCovariance3x3.data[0][0]+=R_left;
Ludwigfr 12:1e80471c5c6c 1541 compositeInnovationCovariance3x3.data[1][1]+=R_front;
Ludwigfr 12:1e80471c5c6c 1542 compositeInnovationCovariance3x3.data[2][2]+=R_right;
Ludwigfr 12:1e80471c5c6c 1543 pc.printf("\r\n111:calcul compositeInnovationCovariance3x3 passed");
Ludwigfr 12:1e80471c5c6c 1544 //get the inverse of the compositeInnovationCovariance
Ludwigfr 12:1e80471c5c6c 1545 compositeInnovationCovariance3x3Inverse.fillWithInverse(compositeInnovationCovariance3x3);
Ludwigfr 12:1e80471c5c6c 1546 pc.printf("\r\n111:calcul compositeInnovationCovariance3x3Inverse passed");
Ludwigfr 12:1e80471c5c6c 1547 //compute KalmanGain=covariancePositionEstimationKplus1K*compositeMeasurementJacobianTranspose*Inverse(compositeInnovationCovariance)
Ludwigfr 12:1e80471c5c6c 1548 tempKalmanGain3X3.fillByMultiplication(covariancePositionEstimationKplus1K,compositeMeasurementJacobian3x3Transpose);
Ludwigfr 12:1e80471c5c6c 1549 kalmanGain3X3.fillByMultiplication(tempKalmanGain3X3,compositeInnovationCovariance3x3Inverse);
Ludwigfr 12:1e80471c5c6c 1550 pc.printf("\r\n111:calcul kalmanGain3X3 passed");
Ludwigfr 12:1e80471c5c6c 1551 //get the transpose of the kalman gain
Ludwigfr 12:1e80471c5c6c 1552 kalmanGain3X3Transpose.fillWithTranspose(kalmanGain3X3);
Ludwigfr 12:1e80471c5c6c 1553 //update pose estimation=old pose estimation + kalman gain*compositeInnovation
Ludwigfr 12:1e80471c5c6c 1554 tempPoseKplus1Kplus1.fillByMultiplication(kalmanGain3X3,compositeInnovation3x1);
Ludwigfr 12:1e80471c5c6c 1555 poseKplus1Kplus1.addition(tempPoseKplus1Kplus1);
Ludwigfr 12:1e80471c5c6c 1556 pc.printf("\r\n111:calcul poseKplus1Kplus1 passed");
Ludwigfr 12:1e80471c5c6c 1557 //compute covariancePositionEstimationKplus1Kplus1=covariancePositionEstimationKplus1K-kalmanGain*compositeInnovationCovariance*KalmanGainTranspose
Ludwigfr 12:1e80471c5c6c 1558 temp2CovariancePositionEstimationKplus1Kplus13x3.fillByMultiplication(kalmanGain3X3,compositeInnovationCovariance3x3);
Ludwigfr 12:1e80471c5c6c 1559 tempCovariancePositionEstimationKplus1Kplus13x3.fillByMultiplication(temp2CovariancePositionEstimationKplus1Kplus13x3,kalmanGain3X3Transpose);
Ludwigfr 12:1e80471c5c6c 1560 covariancePositionEstimationKplus1Kplus1.subtraction(tempCovariancePositionEstimationKplus1Kplus13x3);
Ludwigfr 12:1e80471c5c6c 1561 pc.printf("\r\n111:calcul covariancePositionEstimationKplus1Kplus1 passed");
Ludwigfr 12:1e80471c5c6c 1562 break;
Ludwigfr 12:1e80471c5c6c 1563 }
Ludwigfr 9:1cc27f33d3e1 1564 //update covariancePositionEstimationK =covariancePositionEstimationKplus1Kplus1
Ludwigfr 9:1cc27f33d3e1 1565 this->covariancePositionEstimationK.fillByCopy(covariancePositionEstimationKplus1Kplus1);
Ludwigfr 5:19f24c363418 1566
Ludwigfr 14:696187e74411 1567 ////pc.printf("\r\nposeKplus1Kplus1 X=%f, Y=%f, theta=%f",poseKplus1Kplus1.data[0][0],poseKplus1Kplus1.data[1][0],poseKplus1Kplus1.data[2][0]);
Ludwigfr 9:1cc27f33d3e1 1568 //update pose
Ludwigfr 9:1cc27f33d3e1 1569 this->xWorld=poseKplus1Kplus1.data[0][0];
Ludwigfr 9:1cc27f33d3e1 1570 this->yWorld=poseKplus1Kplus1.data[1][0];
Ludwigfr 9:1cc27f33d3e1 1571 this->thetaWorld=poseKplus1Kplus1.data[2][0];
Ludwigfr 5:19f24c363418 1572
Ludwigfr 14:696187e74411 1573 //pc.printf("\r\nWith Kalman X=%f, Y=%f, theta=%f",xWorld,yWorld,thetaWorld);
Ludwigfr 5:19f24c363418 1574 //try with robot one
Ludwigfr 5:19f24c363418 1575 /*
Ludwigfr 5:19f24c363418 1576 X=xEstimatedKNext;
Ludwigfr 5:19f24c363418 1577 Y=yEstimatedKNext;
Ludwigfr 5:19f24c363418 1578 theta=thetaWorldEstimatedKNext;
Ludwigfr 5:19f24c363418 1579 this->xWorld=-Y;
Ludwigfr 5:19f24c363418 1580 this->yWorld=X;
Ludwigfr 5:19f24c363418 1581 if(theta >PI/2)
Ludwigfr 5:19f24c363418 1582 this->thetaWorld=-PI+(theta-PI/2);
Ludwigfr 5:19f24c363418 1583 else
Ludwigfr 5:19f24c363418 1584 this->thetaWorld=theta+PI/2;
Ludwigfr 5:19f24c363418 1585
Ludwigfr 5:19f24c363418 1586
Ludwigfr 5:19f24c363418 1587 this->print_map_with_robot_position();
Ludwigfr 5:19f24c363418 1588 pc.printf("\n\rX= %f",this->xWorld);
Ludwigfr 5:19f24c363418 1589 pc.printf("\n\rY= %f",this->yWorld);
Ludwigfr 5:19f24c363418 1590 pc.printf("\n\rtheta= %f",this->thetaWorld);
Ludwigfr 5:19f24c363418 1591 */
Ludwigfr 5:19f24c363418 1592 //update odometrie X Y theta...
Ludwigfr 12:1e80471c5c6c 1593 }
Ludwigfr 5:19f24c363418 1594 }
Ludwigfr 5:19f24c363418 1595
Ludwigfr 10:d0109d7cbe7c 1596 void MiniExplorerCoimbra::printMatrix(myMatrix mat1){
Ludwigfr 10:d0109d7cbe7c 1597 for(int i = 0; i < mat1.nbRow; ++i) {
Ludwigfr 10:d0109d7cbe7c 1598 for(int j = 0; j < mat1.nbColumn; ++j) {
Ludwigfr 10:d0109d7cbe7c 1599 pc.printf("\r%f",mat1.data[i][j]);
Ludwigfr 10:d0109d7cbe7c 1600 }
Ludwigfr 10:d0109d7cbe7c 1601 pc.printf("\n");
Ludwigfr 10:d0109d7cbe7c 1602 }
Ludwigfr 12:1e80471c5c6c 1603 pc.printf("\r\n");
Ludwigfr 10:d0109d7cbe7c 1604 }