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@9:1cc27f33d3e1, 2017-07-10 (annotated)

Committer:
Ludwigfr
Date:
Mon Jul 10 16:23:52 2017 +0000
Revision:
9:1cc27f33d3e1
Parent:
8:072a76960e27
Child:
10:d0109d7cbe7c
with lab 4 done with proper matrix function

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