Yo Fleyt / Mbed 2 deprecated roboticLab_withclass

lab robotic coimbra

Dependencies:   ISR_Mini-explorer mbed

MiniExplorerCoimbra.cpp@0:9f7ee7ed13e4, 2017-06-26 (annotated)

Committer:
Ludwigfr
Date:
Mon Jun 26 12:05:20 2017 +0000
Revision:
0:9f7ee7ed13e4
this version should work, though it would be nice to test all the lab demo; there's also some code on the 4th lab at the end of MiniExplorerCoimbra.cpp

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 0:9f7ee7ed13e4 6 MiniExplorerCoimbra::MiniExplorerCoimbra(float defaultXWorld, float defaultYWorld, float defaultThetaWorld):map(200,200,20,20),sonarLeft(10*PI/36,-4,4),sonarFront(0,0,5),sonarRight(-10*PI/36,4,4){
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 0:9f7ee7ed13e4 17 this->khro=12;
Ludwigfr 0:9f7ee7ed13e4 18 this->ka=30;
Ludwigfr 0:9f7ee7ed13e4 19 this->kb=-13;
Ludwigfr 0:9f7ee7ed13e4 20 this->kv=200;
Ludwigfr 0:9f7ee7ed13e4 21 this->kh=200;
Ludwigfr 0:9f7ee7ed13e4 22
Ludwigfr 0:9f7ee7ed13e4 23 this->rangeForce=30;
Ludwigfr 0:9f7ee7ed13e4 24 this->attractionConstantForce=10000;
Ludwigfr 0:9f7ee7ed13e4 25 this->repulsionConstantForce=1;
Ludwigfr 0:9f7ee7ed13e4 26 }
Ludwigfr 0:9f7ee7ed13e4 27
Ludwigfr 0:9f7ee7ed13e4 28 void MiniExplorerCoimbra::setXYThetaAndXYThetaWorld(float defaultXWorld, float defaultYWorld, float defaultThetaWorld){
Ludwigfr 0:9f7ee7ed13e4 29 this->xWorld=defaultXWorld;
Ludwigfr 0:9f7ee7ed13e4 30 this->yWorld=defaultYWorld;
Ludwigfr 0:9f7ee7ed13e4 31 this->thetaWorld=defaultThetaWorld;
Ludwigfr 0:9f7ee7ed13e4 32 X=defaultYWorld;
Ludwigfr 0:9f7ee7ed13e4 33 Y=-defaultXWorld;
Ludwigfr 0:9f7ee7ed13e4 34 if(defaultThetaWorld < -PI/2)
Ludwigfr 0:9f7ee7ed13e4 35 theta=PI/2+PI-defaultThetaWorld;
Ludwigfr 0:9f7ee7ed13e4 36 else
Ludwigfr 0:9f7ee7ed13e4 37 theta=defaultThetaWorld-PI/2;
Ludwigfr 0:9f7ee7ed13e4 38 }
Ludwigfr 0:9f7ee7ed13e4 39
Ludwigfr 0:9f7ee7ed13e4 40 void MiniExplorerCoimbra::myOdometria(){
Ludwigfr 0:9f7ee7ed13e4 41 Odometria();
Ludwigfr 0:9f7ee7ed13e4 42 this->xWorld=-Y;
Ludwigfr 0:9f7ee7ed13e4 43 this->yWorld=X;
Ludwigfr 0:9f7ee7ed13e4 44 if(theta >PI/2)
Ludwigfr 0:9f7ee7ed13e4 45 this->thetaWorld=-PI+(theta-PI/2);
Ludwigfr 0:9f7ee7ed13e4 46 else
Ludwigfr 0:9f7ee7ed13e4 47 this->thetaWorld=theta+PI/2;
Ludwigfr 0:9f7ee7ed13e4 48 }
Ludwigfr 0:9f7ee7ed13e4 49
Ludwigfr 0:9f7ee7ed13e4 50 //generate a position randomly and makes the robot go there while updating the map
Ludwigfr 0:9f7ee7ed13e4 51 void MiniExplorerCoimbra::randomize_and_map() {
Ludwigfr 0:9f7ee7ed13e4 52 //TODO check that it's aurelien's work
Ludwigfr 0:9f7ee7ed13e4 53 float movementOnX=rand()%(int)(this->map.widthRealMap/2);
Ludwigfr 0:9f7ee7ed13e4 54 float movementOnY=rand()%(int)(this->map.heightRealMap/2);
Ludwigfr 0:9f7ee7ed13e4 55
Ludwigfr 0:9f7ee7ed13e4 56 float signOfX=rand()%2;
Ludwigfr 0:9f7ee7ed13e4 57 if(signOfX < 1)
Ludwigfr 0:9f7ee7ed13e4 58 signOfX=-1;
Ludwigfr 0:9f7ee7ed13e4 59 float signOfY=rand()%2;
Ludwigfr 0:9f7ee7ed13e4 60 if(signOfY < 1)
Ludwigfr 0:9f7ee7ed13e4 61 signOfY=-1;
Ludwigfr 0:9f7ee7ed13e4 62
Ludwigfr 0:9f7ee7ed13e4 63 float targetXWorld = movementOnX*signOfX;
Ludwigfr 0:9f7ee7ed13e4 64 float targetYWorld = movementOnY*signOfY;
Ludwigfr 0:9f7ee7ed13e4 65 float targetAngleWorld = 2*((float)(rand()%31416)-15708)/10000.0;
Ludwigfr 0:9f7ee7ed13e4 66 this->go_to_point_with_angle(targetXWorld, targetYWorld, targetAngleWorld);
Ludwigfr 0:9f7ee7ed13e4 67 }
Ludwigfr 0:9f7ee7ed13e4 68
Ludwigfr 0:9f7ee7ed13e4 69 //move of targetXWorld and targetYWorld ending in a targetAngleWorld
Ludwigfr 0:9f7ee7ed13e4 70 void MiniExplorerCoimbra::go_to_point_with_angle(float targetXWorld, float targetYWorld, float targetAngleWorld) {
Ludwigfr 0:9f7ee7ed13e4 71 bool keepGoing=true;
Ludwigfr 0:9f7ee7ed13e4 72 float leftMm;
Ludwigfr 0:9f7ee7ed13e4 73 float frontMm;
Ludwigfr 0:9f7ee7ed13e4 74 float rightMm;
Ludwigfr 0:9f7ee7ed13e4 75 float dt;
Ludwigfr 0:9f7ee7ed13e4 76 Timer t;
Ludwigfr 0:9f7ee7ed13e4 77 float distanceToTarget;
Ludwigfr 0:9f7ee7ed13e4 78 do {
Ludwigfr 0:9f7ee7ed13e4 79 //Timer stuff
Ludwigfr 0:9f7ee7ed13e4 80 dt = t.read();
Ludwigfr 0:9f7ee7ed13e4 81 t.reset();
Ludwigfr 0:9f7ee7ed13e4 82 t.start();
Ludwigfr 0:9f7ee7ed13e4 83
Ludwigfr 0:9f7ee7ed13e4 84 //Updating X,Y and theta with the odometry values
Ludwigfr 0:9f7ee7ed13e4 85 this->myOdometria();
Ludwigfr 0:9f7ee7ed13e4 86 leftMm = get_distance_left_sensor();
Ludwigfr 0:9f7ee7ed13e4 87 frontMm = get_distance_front_sensor();
Ludwigfr 0:9f7ee7ed13e4 88 rightMm = get_distance_right_sensor();
Ludwigfr 0:9f7ee7ed13e4 89
Ludwigfr 0:9f7ee7ed13e4 90 //if in dangerzone
Ludwigfr 0:9f7ee7ed13e4 91 if((frontMm < 150 && frontMm > 0)|| (leftMm <150 && leftMm > 0) || (rightMm <150 && rightMm > 0) ){
Ludwigfr 0:9f7ee7ed13e4 92 leftMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 93 rightMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 94 this->update_sonar_values(leftMm, frontMm, rightMm);
Ludwigfr 0:9f7ee7ed13e4 95 //TODO Giorgos maybe you can also test the do_half_flip() function
Ludwigfr 0:9f7ee7ed13e4 96 this->myOdometria();
Ludwigfr 0:9f7ee7ed13e4 97 //do a flip TODO
Ludwigfr 0:9f7ee7ed13e4 98 keepGoing=false;
Ludwigfr 0:9f7ee7ed13e4 99 this->do_half_flip();
Ludwigfr 0:9f7ee7ed13e4 100 }else{
Ludwigfr 0:9f7ee7ed13e4 101 //if not in danger zone continue as usual
Ludwigfr 0:9f7ee7ed13e4 102 this->update_sonar_values(leftMm, frontMm, rightMm);
Ludwigfr 0:9f7ee7ed13e4 103
Ludwigfr 0:9f7ee7ed13e4 104 //Updating motor velocities
Ludwigfr 0:9f7ee7ed13e4 105 distanceToTarget=this->update_angular_speed_wheels_go_to_point_with_angle(targetXWorld,targetYWorld,targetAngleWorld,dt);
Ludwigfr 0:9f7ee7ed13e4 106
Ludwigfr 0:9f7ee7ed13e4 107 wait(0.2);
Ludwigfr 0:9f7ee7ed13e4 108 //Timer stuff
Ludwigfr 0:9f7ee7ed13e4 109 t.stop();
Ludwigfr 0:9f7ee7ed13e4 110 pc.printf("\n\r dist to target= %f",distanceToTarget);
Ludwigfr 0:9f7ee7ed13e4 111 }
Ludwigfr 0:9f7ee7ed13e4 112 } while(distanceToTarget>1 && (abs(targetAngleWorld-this->thetaWorld)>0.01) && keepGoing);
Ludwigfr 0:9f7ee7ed13e4 113
Ludwigfr 0:9f7ee7ed13e4 114 //Stop at the end
Ludwigfr 0:9f7ee7ed13e4 115 leftMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 116 rightMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 117 }
Ludwigfr 0:9f7ee7ed13e4 118
Ludwigfr 0:9f7ee7ed13e4 119 float MiniExplorerCoimbra::update_angular_speed_wheels_go_to_point_with_angle(float targetXWorld, float targetYWorld, float targetAngleWorld, float dt){
Ludwigfr 0:9f7ee7ed13e4 120 //compute_angles_and_distance
Ludwigfr 0:9f7ee7ed13e4 121 //atan2 take the deplacement on x and the deplacement on y as parameters
Ludwigfr 0:9f7ee7ed13e4 122 float angleToPoint = atan2((targetYWorld-this->yWorld),(targetXWorld-this->xWorld))-this->thetaWorld;
Ludwigfr 0:9f7ee7ed13e4 123 angleToPoint = atan(sin(angleToPoint)/cos(angleToPoint));
Ludwigfr 0:9f7ee7ed13e4 124 //rho is the distance to the point of arrival
Ludwigfr 0:9f7ee7ed13e4 125 float rho = dist(targetXWorld,targetYWorld,this->xWorld,this->yWorld);
Ludwigfr 0:9f7ee7ed13e4 126 float distanceToTarget = rho;
Ludwigfr 0:9f7ee7ed13e4 127 //TODO check that
Ludwigfr 0:9f7ee7ed13e4 128 float beta = targetAngleWorld-angleToPoint-this->thetaWorld;
Ludwigfr 0:9f7ee7ed13e4 129
Ludwigfr 0:9f7ee7ed13e4 130 //Computing angle error and distance towards the target value
Ludwigfr 0:9f7ee7ed13e4 131 rho += dt*(-this->khro*cos(angleToPoint)*rho);
Ludwigfr 0:9f7ee7ed13e4 132 float temp = angleToPoint;
Ludwigfr 0:9f7ee7ed13e4 133 angleToPoint += dt*(this->khro*sin(angleToPoint)-this->ka*angleToPoint-this->kb*beta);
Ludwigfr 0:9f7ee7ed13e4 134 beta += dt*(-this->khro*sin(temp));
Ludwigfr 0:9f7ee7ed13e4 135
Ludwigfr 0:9f7ee7ed13e4 136 //Computing linear and angular velocities
Ludwigfr 0:9f7ee7ed13e4 137 float linear;
Ludwigfr 0:9f7ee7ed13e4 138 float angular;
Ludwigfr 0:9f7ee7ed13e4 139 if(angleToPoint>=-1.5708 && angleToPoint<=1.5708){
Ludwigfr 0:9f7ee7ed13e4 140 linear=this->khro*rho;
Ludwigfr 0:9f7ee7ed13e4 141 angular=this->ka*angleToPoint+this->kb*beta;
Ludwigfr 0:9f7ee7ed13e4 142 }
Ludwigfr 0:9f7ee7ed13e4 143 else{
Ludwigfr 0:9f7ee7ed13e4 144 linear=-this->khro*rho;
Ludwigfr 0:9f7ee7ed13e4 145 angular=-this->ka*angleToPoint-this->kb*beta;
Ludwigfr 0:9f7ee7ed13e4 146 }
Ludwigfr 0:9f7ee7ed13e4 147 float angular_left=(linear-0.5*this->distanceWheels*angular)/this->radiusWheels;
Ludwigfr 0:9f7ee7ed13e4 148 float angular_right=(linear+0.5*this->distanceWheels*angular)/this->radiusWheels;
Ludwigfr 0:9f7ee7ed13e4 149
Ludwigfr 0:9f7ee7ed13e4 150 //Normalize speed for motors
Ludwigfr 0:9f7ee7ed13e4 151 if(angular_left>angular_right) {
Ludwigfr 0:9f7ee7ed13e4 152 angular_right=this->speed*angular_right/angular_left;
Ludwigfr 0:9f7ee7ed13e4 153 angular_left=this->speed;
Ludwigfr 0:9f7ee7ed13e4 154 } else {
Ludwigfr 0:9f7ee7ed13e4 155 angular_left=this->speed*angular_left/angular_right;
Ludwigfr 0:9f7ee7ed13e4 156 angular_right=this->speed;
Ludwigfr 0:9f7ee7ed13e4 157 }
Ludwigfr 0:9f7ee7ed13e4 158
Ludwigfr 0:9f7ee7ed13e4 159 //compute_linear_angular_velocities
Ludwigfr 0:9f7ee7ed13e4 160 leftMotor(1,angular_left);
Ludwigfr 0:9f7ee7ed13e4 161 rightMotor(1,angular_right);
Ludwigfr 0:9f7ee7ed13e4 162
Ludwigfr 0:9f7ee7ed13e4 163 return distanceToTarget;
Ludwigfr 0:9f7ee7ed13e4 164 }
Ludwigfr 0:9f7ee7ed13e4 165
Ludwigfr 0:9f7ee7ed13e4 166 void MiniExplorerCoimbra::update_sonar_values(float leftMm,float frontMm,float rightMm){
Ludwigfr 0:9f7ee7ed13e4 167 float xWorldCell;
Ludwigfr 0:9f7ee7ed13e4 168 float yWorldCell;
Ludwigfr 0:9f7ee7ed13e4 169 for(int i=0;i<this->map.nbCellWidth;i++){
Ludwigfr 0:9f7ee7ed13e4 170 for(int j=0;j<this->map.nbCellHeight;j++){
Ludwigfr 0:9f7ee7ed13e4 171 xWorldCell=this->map.cell_width_coordinate_to_world(i);
Ludwigfr 0:9f7ee7ed13e4 172 yWorldCell=this->map.cell_height_coordinate_to_world(j);
Ludwigfr 0:9f7ee7ed13e4 173 this->map.update_cell_value(i,j,this->sonarRight.compute_probability_t(rightMm/10,xWorldCell,yWorldCell,this->xWorld,this->yWorld,this->thetaWorld));
Ludwigfr 0:9f7ee7ed13e4 174 this->map.update_cell_value(i,j,this->sonarLeft.compute_probability_t(leftMm/10,xWorldCell,yWorldCell,this->xWorld,this->yWorld,this->thetaWorld));
Ludwigfr 0:9f7ee7ed13e4 175 this->map.update_cell_value(i,j,this->sonarFront.compute_probability_t(frontMm/10,xWorldCell,yWorldCell,this->xWorld,this->yWorld,this->thetaWorld));
Ludwigfr 0:9f7ee7ed13e4 176
Ludwigfr 0:9f7ee7ed13e4 177 }
Ludwigfr 0:9f7ee7ed13e4 178 }
Ludwigfr 0:9f7ee7ed13e4 179 }
Ludwigfr 0:9f7ee7ed13e4 180
Ludwigfr 0:9f7ee7ed13e4 181 void MiniExplorerCoimbra::do_half_flip(){
Ludwigfr 0:9f7ee7ed13e4 182 this->myOdometria();
Ludwigfr 0:9f7ee7ed13e4 183 float theta_plus_h_pi=theta+PI/2;//theta is between -PI and PI
Ludwigfr 0:9f7ee7ed13e4 184 if(theta_plus_h_pi > PI)
Ludwigfr 0:9f7ee7ed13e4 185 theta_plus_h_pi=-(2*PI-theta_plus_h_pi);
Ludwigfr 0:9f7ee7ed13e4 186 leftMotor(0,100);
Ludwigfr 0:9f7ee7ed13e4 187 rightMotor(1,100);
Ludwigfr 0:9f7ee7ed13e4 188 while(abs(theta_plus_h_pi-theta)>0.05){
Ludwigfr 0:9f7ee7ed13e4 189 this->myOdometria();
Ludwigfr 0:9f7ee7ed13e4 190 // pc.printf("\n\r diff=%f", abs(theta_plus_pi-theta));
Ludwigfr 0:9f7ee7ed13e4 191 }
Ludwigfr 0:9f7ee7ed13e4 192 leftMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 193 rightMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 194 }
Ludwigfr 0:9f7ee7ed13e4 195
Ludwigfr 0:9f7ee7ed13e4 196 //Distance computation function
Ludwigfr 0:9f7ee7ed13e4 197 float MiniExplorerCoimbra::dist(float x1, float y1, float x2, float y2){
Ludwigfr 0:9f7ee7ed13e4 198 return sqrt(pow(y2-y1,2) + pow(x2-x1,2));
Ludwigfr 0:9f7ee7ed13e4 199 }
Ludwigfr 0:9f7ee7ed13e4 200
Ludwigfr 0:9f7ee7ed13e4 201 //use virtual force field
Ludwigfr 0:9f7ee7ed13e4 202 void MiniExplorerCoimbra::try_to_reach_target(float targetXWorld,float targetYWorld){
Ludwigfr 0:9f7ee7ed13e4 203 //atan2 gives the angle beetween PI and -PI
Ludwigfr 0:9f7ee7ed13e4 204 this->myOdometria();
Ludwigfr 0:9f7ee7ed13e4 205 /*
Ludwigfr 0:9f7ee7ed13e4 206 float deplacementOnXWorld=targetXWorld-this->xWorld;
Ludwigfr 0:9f7ee7ed13e4 207 float deplacementOnYWorld=targetYWorld-this->yWorld;
Ludwigfr 0:9f7ee7ed13e4 208 */
Ludwigfr 0:9f7ee7ed13e4 209 float angleToTarget=atan2(targetYWorld-this->yWorld,targetXWorld-this->xWorld);
Ludwigfr 0:9f7ee7ed13e4 210 turn_to_target(angleToTarget);
Ludwigfr 0:9f7ee7ed13e4 211 bool reached=false;
Ludwigfr 0:9f7ee7ed13e4 212 int print=0;
Ludwigfr 0:9f7ee7ed13e4 213 while (!reached) {
Ludwigfr 0:9f7ee7ed13e4 214 this->vff(&reached,targetXWorld,targetYWorld);
Ludwigfr 0:9f7ee7ed13e4 215 //test_got_to_line(&reached);
Ludwigfr 0:9f7ee7ed13e4 216 if(print==10){
Ludwigfr 0:9f7ee7ed13e4 217 leftMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 218 rightMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 219 this->print_map_with_robot_position_and_target(targetXWorld,targetYWorld);
Ludwigfr 0:9f7ee7ed13e4 220 print=0;
Ludwigfr 0:9f7ee7ed13e4 221 }else
Ludwigfr 0:9f7ee7ed13e4 222 print+=1;
Ludwigfr 0:9f7ee7ed13e4 223 }
Ludwigfr 0:9f7ee7ed13e4 224 //Stop at the end
Ludwigfr 0:9f7ee7ed13e4 225 leftMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 226 rightMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 227 pc.printf("\r\n target reached");
Ludwigfr 0:9f7ee7ed13e4 228 wait(3);//
Ludwigfr 0:9f7ee7ed13e4 229 }
Ludwigfr 0:9f7ee7ed13e4 230
Ludwigfr 0:9f7ee7ed13e4 231 void MiniExplorerCoimbra::vff(bool* reached, float targetXWorld, float targetYWorld){
Ludwigfr 0:9f7ee7ed13e4 232 float line_a;
Ludwigfr 0:9f7ee7ed13e4 233 float line_b;
Ludwigfr 0:9f7ee7ed13e4 234 float line_c;
Ludwigfr 0:9f7ee7ed13e4 235 //Updating X,Y and theta with the odometry values
Ludwigfr 0:9f7ee7ed13e4 236 float forceXWorld=0;
Ludwigfr 0:9f7ee7ed13e4 237 float forceYWorld=0;
Ludwigfr 0:9f7ee7ed13e4 238 //we update the odometrie
Ludwigfr 0:9f7ee7ed13e4 239 this->myOdometria();
Ludwigfr 0:9f7ee7ed13e4 240 //we check the sensors
Ludwigfr 0:9f7ee7ed13e4 241 float leftMm = get_distance_left_sensor();
Ludwigfr 0:9f7ee7ed13e4 242 float frontMm = get_distance_front_sensor();
Ludwigfr 0:9f7ee7ed13e4 243 float rightMm = get_distance_right_sensor();
Ludwigfr 0:9f7ee7ed13e4 244 //update the probabilities values
Ludwigfr 0:9f7ee7ed13e4 245 this->update_sonar_values(leftMm, frontMm, rightMm);
Ludwigfr 0:9f7ee7ed13e4 246 //we compute the force on X and Y
Ludwigfr 0:9f7ee7ed13e4 247 this->compute_forceX_and_forceY(&forceXWorld, &forceYWorld,targetXWorld,targetYWorld);
Ludwigfr 0:9f7ee7ed13e4 248 //we compute a new ine
Ludwigfr 0:9f7ee7ed13e4 249 this->calculate_line(forceXWorld, forceYWorld, &line_a,&line_b,&line_c);
Ludwigfr 0:9f7ee7ed13e4 250 //Updating motor velocities
Ludwigfr 0:9f7ee7ed13e4 251 this->go_to_line(line_a,line_b,line_c,targetXWorld,targetYWorld);
Ludwigfr 0:9f7ee7ed13e4 252
Ludwigfr 0:9f7ee7ed13e4 253 //wait(0.1);
Ludwigfr 0:9f7ee7ed13e4 254 this->myOdometria();
Ludwigfr 0:9f7ee7ed13e4 255 if(dist(this->xWorld,this->yWorld,targetXWorld,targetYWorld)<10)
Ludwigfr 0:9f7ee7ed13e4 256 *reached=true;
Ludwigfr 0:9f7ee7ed13e4 257 }
Ludwigfr 0:9f7ee7ed13e4 258
Ludwigfr 0:9f7ee7ed13e4 259 /*angleToTarget is obtained through atan2 so it s:
Ludwigfr 0:9f7ee7ed13e4 260 < 0 if the angle is bettween PI and 2pi on a trigo circle
Ludwigfr 0:9f7ee7ed13e4 261 > 0 if it is between 0 and PI
Ludwigfr 0:9f7ee7ed13e4 262 */
Ludwigfr 0:9f7ee7ed13e4 263 void MiniExplorerCoimbra::turn_to_target(float angleToTarget){
Ludwigfr 0:9f7ee7ed13e4 264 this->myOdometria();
Ludwigfr 0:9f7ee7ed13e4 265 float theta_plus_h_pi=theta+PI/2;//theta is between -PI and PI
Ludwigfr 0:9f7ee7ed13e4 266 if(theta_plus_h_pi > PI)
Ludwigfr 0:9f7ee7ed13e4 267 theta_plus_h_pi=-(2*PI-theta_plus_h_pi);
Ludwigfr 0:9f7ee7ed13e4 268 if(angleToTarget>0){
Ludwigfr 0:9f7ee7ed13e4 269 leftMotor(0,1);
Ludwigfr 0:9f7ee7ed13e4 270 rightMotor(1,1);
Ludwigfr 0:9f7ee7ed13e4 271 }else{
Ludwigfr 0:9f7ee7ed13e4 272 leftMotor(1,1);
Ludwigfr 0:9f7ee7ed13e4 273 rightMotor(0,1);
Ludwigfr 0:9f7ee7ed13e4 274 }
Ludwigfr 0:9f7ee7ed13e4 275 while(abs(angleToTarget-theta_plus_h_pi)>0.05){
Ludwigfr 0:9f7ee7ed13e4 276 this->myOdometria();
Ludwigfr 0:9f7ee7ed13e4 277 theta_plus_h_pi=theta+PI/2;//theta is between -PI and PI
Ludwigfr 0:9f7ee7ed13e4 278 if(theta_plus_h_pi > PI)
Ludwigfr 0:9f7ee7ed13e4 279 theta_plus_h_pi=-(2*PI-theta_plus_h_pi);
Ludwigfr 0:9f7ee7ed13e4 280 //pc.printf("\n\r diff=%f", abs(angleToTarget-theta_plus_h_pi));
Ludwigfr 0:9f7ee7ed13e4 281 }
Ludwigfr 0:9f7ee7ed13e4 282 leftMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 283 rightMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 284 }
Ludwigfr 0:9f7ee7ed13e4 285
Ludwigfr 0:9f7ee7ed13e4 286
Ludwigfr 0:9f7ee7ed13e4 287 void MiniExplorerCoimbra::print_map_with_robot_position_and_target(float targetXWorld, float targetYWorld) {
Ludwigfr 0:9f7ee7ed13e4 288 float currProba;
Ludwigfr 0:9f7ee7ed13e4 289
Ludwigfr 0:9f7ee7ed13e4 290 float heightIndiceInOrthonormal;
Ludwigfr 0:9f7ee7ed13e4 291 float widthIndiceInOrthonormal;
Ludwigfr 0:9f7ee7ed13e4 292
Ludwigfr 0:9f7ee7ed13e4 293 float widthMalus=-(3*this->map.sizeCellWidth/2);
Ludwigfr 0:9f7ee7ed13e4 294 float widthBonus=this->map.sizeCellWidth/2;
Ludwigfr 0:9f7ee7ed13e4 295
Ludwigfr 0:9f7ee7ed13e4 296 float heightMalus=-(3*this->map.sizeCellHeight/2);
Ludwigfr 0:9f7ee7ed13e4 297 float heightBonus=this->map.sizeCellHeight/2;
Ludwigfr 0:9f7ee7ed13e4 298
Ludwigfr 0:9f7ee7ed13e4 299 pc.printf("\n\r");
Ludwigfr 0:9f7ee7ed13e4 300 for (int y = this->map.nbCellHeight -1; y>-1; y--) {
Ludwigfr 0:9f7ee7ed13e4 301 for (int x= 0; x<this->map.nbCellWidth; x++) {
Ludwigfr 0:9f7ee7ed13e4 302 heightIndiceInOrthonormal=this->map.cell_height_coordinate_to_world(y);
Ludwigfr 0:9f7ee7ed13e4 303 widthIndiceInOrthonormal=this->map.cell_width_coordinate_to_world(x);
Ludwigfr 0:9f7ee7ed13e4 304 if(this->yWorld >= (heightIndiceInOrthonormal+heightMalus) && this->yWorld <= (heightIndiceInOrthonormal+heightBonus) && this->xWorld >= (widthIndiceInOrthonormal+widthMalus) && this->xWorld <= (widthIndiceInOrthonormal+widthBonus))
Ludwigfr 0:9f7ee7ed13e4 305 pc.printf(" R ");
Ludwigfr 0:9f7ee7ed13e4 306 else{
Ludwigfr 0:9f7ee7ed13e4 307 if(targetYWorld >= (heightIndiceInOrthonormal+heightMalus) && targetYWorld <= (heightIndiceInOrthonormal+heightBonus) && targetXWorld >= (widthIndiceInOrthonormal+widthMalus) && targetXWorld <= (widthIndiceInOrthonormal+widthBonus))
Ludwigfr 0:9f7ee7ed13e4 308 pc.printf(" T ");
Ludwigfr 0:9f7ee7ed13e4 309 else{
Ludwigfr 0:9f7ee7ed13e4 310 currProba=this->map.log_to_proba(this->map.cellsLogValues[x][y]);
Ludwigfr 0:9f7ee7ed13e4 311 if ( currProba < 0.5)
Ludwigfr 0:9f7ee7ed13e4 312 pc.printf(" ");
Ludwigfr 0:9f7ee7ed13e4 313 else{
Ludwigfr 0:9f7ee7ed13e4 314 if(currProba==0.5)
Ludwigfr 0:9f7ee7ed13e4 315 pc.printf(" . ");
Ludwigfr 0:9f7ee7ed13e4 316 else
Ludwigfr 0:9f7ee7ed13e4 317 pc.printf(" X ");
Ludwigfr 0:9f7ee7ed13e4 318 }
Ludwigfr 0:9f7ee7ed13e4 319 }
Ludwigfr 0:9f7ee7ed13e4 320 }
Ludwigfr 0:9f7ee7ed13e4 321 }
Ludwigfr 0:9f7ee7ed13e4 322 pc.printf("\n\r");
Ludwigfr 0:9f7ee7ed13e4 323 }
Ludwigfr 0:9f7ee7ed13e4 324 }
Ludwigfr 0:9f7ee7ed13e4 325
Ludwigfr 0:9f7ee7ed13e4 326
Ludwigfr 0:9f7ee7ed13e4 327 //robotX and robotY are from this->myOdometria(), calculate line_a, line_b and line_c
Ludwigfr 0:9f7ee7ed13e4 328 void MiniExplorerCoimbra::calculate_line(float forceX, float forceY, float *line_a, float *line_b, float *line_c){
Ludwigfr 0:9f7ee7ed13e4 329 /*
Ludwigfr 0:9f7ee7ed13e4 330 in the teachers maths it is
Ludwigfr 0:9f7ee7ed13e4 331
Ludwigfr 0:9f7ee7ed13e4 332 *line_a=forceY;
Ludwigfr 0:9f7ee7ed13e4 333 *line_b=-forceX;
Ludwigfr 0:9f7ee7ed13e4 334
Ludwigfr 0:9f7ee7ed13e4 335 because a*x+b*y+c=0
Ludwigfr 0:9f7ee7ed13e4 336 a impact the vertical and b the horizontal
Ludwigfr 0:9f7ee7ed13e4 337 and he has to put them like this because
Ludwigfr 0:9f7ee7ed13e4 338 Robot space: World space:
Ludwigfr 0:9f7ee7ed13e4 339 ^ ^
Ludwigfr 0:9f7ee7ed13e4 340 |x |y
Ludwigfr 0:9f7ee7ed13e4 341 <- R O ->
Ludwigfr 0:9f7ee7ed13e4 342 y x
Ludwigfr 0:9f7ee7ed13e4 343 but since our forceX, forceY are already computed in the orthonormal space I m not sure we need to
Ludwigfr 0:9f7ee7ed13e4 344 */
Ludwigfr 0:9f7ee7ed13e4 345 *line_a=forceX;
Ludwigfr 0:9f7ee7ed13e4 346 *line_b=forceY;
Ludwigfr 0:9f7ee7ed13e4 347 //because the line computed always pass by the robot center we dont need lince_c
Ludwigfr 0:9f7ee7ed13e4 348 //*line_c=forceX*this->yWorld+forceY*this->xWorld;
Ludwigfr 0:9f7ee7ed13e4 349 *line_c=0;
Ludwigfr 0:9f7ee7ed13e4 350 }
Ludwigfr 0:9f7ee7ed13e4 351 //compute the force on X and Y
Ludwigfr 0:9f7ee7ed13e4 352 void MiniExplorerCoimbra::compute_forceX_and_forceY(float* forceXWorld, float* forceYWorld, float targetXWorld, float targetYWorld){
Ludwigfr 0:9f7ee7ed13e4 353 float forceRepulsionComputedX=0;
Ludwigfr 0:9f7ee7ed13e4 354 float forceRepulsionComputedY=0;
Ludwigfr 0:9f7ee7ed13e4 355 for(int i=0;i<this->map.nbCellWidth;i++){ //for each cell of the map we compute a force of repulsion
Ludwigfr 0:9f7ee7ed13e4 356 for(int j=0;j<this->map.nbCellHeight;j++){
Ludwigfr 0:9f7ee7ed13e4 357 this->update_force(i,j,&forceRepulsionComputedX,&forceRepulsionComputedY);
Ludwigfr 0:9f7ee7ed13e4 358 }
Ludwigfr 0:9f7ee7ed13e4 359 }
Ludwigfr 0:9f7ee7ed13e4 360 //update with attraction force
Ludwigfr 0:9f7ee7ed13e4 361 *forceXWorld=+forceRepulsionComputedX;
Ludwigfr 0:9f7ee7ed13e4 362 *forceYWorld=+forceRepulsionComputedY;
Ludwigfr 0:9f7ee7ed13e4 363 float distanceTargetRobot=sqrt(pow(targetXWorld-this->xWorld,2)+pow(targetYWorld-this->yWorld,2));
Ludwigfr 0:9f7ee7ed13e4 364 if(distanceTargetRobot != 0){
Ludwigfr 0:9f7ee7ed13e4 365 *forceXWorld-=this->attractionConstantForce*(targetXWorld-this->xWorld)/distanceTargetRobot;
Ludwigfr 0:9f7ee7ed13e4 366 *forceYWorld-=this->attractionConstantForce*(targetYWorld-this->yWorld)/distanceTargetRobot;
Ludwigfr 0:9f7ee7ed13e4 367 }
Ludwigfr 0:9f7ee7ed13e4 368 float amplitude=sqrt(pow(*forceXWorld,2)+pow(*forceYWorld,2));
Ludwigfr 0:9f7ee7ed13e4 369 if(amplitude!=0){//avoid division by 0 if forceX and forceY == 0
Ludwigfr 0:9f7ee7ed13e4 370 *forceXWorld=*forceXWorld/amplitude;
Ludwigfr 0:9f7ee7ed13e4 371 *forceYWorld=*forceYWorld/amplitude;
Ludwigfr 0:9f7ee7ed13e4 372 }
Ludwigfr 0:9f7ee7ed13e4 373 }
Ludwigfr 0:9f7ee7ed13e4 374
Ludwigfr 0:9f7ee7ed13e4 375 //currently line_c is not used
Ludwigfr 0:9f7ee7ed13e4 376 void MiniExplorerCoimbra::go_to_line(float line_a, float line_b, float line_c,float targetXWorld, float targetYWorld){
Ludwigfr 0:9f7ee7ed13e4 377 float lineAngle;
Ludwigfr 0:9f7ee7ed13e4 378 float angleError;
Ludwigfr 0:9f7ee7ed13e4 379 float linear;
Ludwigfr 0:9f7ee7ed13e4 380 float angular;
Ludwigfr 0:9f7ee7ed13e4 381
Ludwigfr 0:9f7ee7ed13e4 382 //atan2 use the deplacement on X and the deplacement on Y
Ludwigfr 0:9f7ee7ed13e4 383 float angleToTarget=atan2(targetYWorld-this->yWorld,targetXWorld-this->xWorld);
Ludwigfr 0:9f7ee7ed13e4 384 bool aligned=false;
Ludwigfr 0:9f7ee7ed13e4 385
Ludwigfr 0:9f7ee7ed13e4 386 //this condition is passed if the target is in the same direction as the robot orientation
Ludwigfr 0:9f7ee7ed13e4 387 if((angleToTarget>0 && this->thetaWorld > 0) || (angleToTarget<0 && this->thetaWorld <0))
Ludwigfr 0:9f7ee7ed13e4 388 aligned=true;
Ludwigfr 0:9f7ee7ed13e4 389
Ludwigfr 0:9f7ee7ed13e4 390 //line angle is beetween pi/2 and -pi/2
Ludwigfr 0:9f7ee7ed13e4 391 /*
Ludwigfr 0:9f7ee7ed13e4 392 ax+by+c=0 (here c==0)
Ludwigfr 0:9f7ee7ed13e4 393 y=x*-a/b
Ludwigfr 0:9f7ee7ed13e4 394 if a*b > 0, the robot is going down
Ludwigfr 0:9f7ee7ed13e4 395 if a*b <0, the robot is going up
Ludwigfr 0:9f7ee7ed13e4 396 */
Ludwigfr 0:9f7ee7ed13e4 397 if(line_b!=0){
Ludwigfr 0:9f7ee7ed13e4 398 if(!aligned)
Ludwigfr 0:9f7ee7ed13e4 399 lineAngle=atan(-line_a/line_b);
Ludwigfr 0:9f7ee7ed13e4 400 else
Ludwigfr 0:9f7ee7ed13e4 401 lineAngle=atan(line_a/-line_b);
Ludwigfr 0:9f7ee7ed13e4 402 }
Ludwigfr 0:9f7ee7ed13e4 403 else{
Ludwigfr 0:9f7ee7ed13e4 404 lineAngle=1.5708;
Ludwigfr 0:9f7ee7ed13e4 405 }
Ludwigfr 0:9f7ee7ed13e4 406
Ludwigfr 0:9f7ee7ed13e4 407 //Computing angle error
Ludwigfr 0:9f7ee7ed13e4 408 angleError = lineAngle-this->thetaWorld;//TODO that I m not sure
Ludwigfr 0:9f7ee7ed13e4 409 angleError = atan(sin(angleError)/cos(angleError));
Ludwigfr 0:9f7ee7ed13e4 410
Ludwigfr 0:9f7ee7ed13e4 411 //Calculating velocities
Ludwigfr 0:9f7ee7ed13e4 412 linear=this->kv*(3.1416);
Ludwigfr 0:9f7ee7ed13e4 413 angular=this->kh*angleError;
Ludwigfr 0:9f7ee7ed13e4 414
Ludwigfr 0:9f7ee7ed13e4 415 float angularLeft=(linear-0.5*this->distanceWheels*angular)/this->radiusWheels;
Ludwigfr 0:9f7ee7ed13e4 416 float angularRight=(linear+0.5*this->distanceWheels*angular)/this->radiusWheels;
Ludwigfr 0:9f7ee7ed13e4 417
Ludwigfr 0:9f7ee7ed13e4 418 //Normalize speed for motors
Ludwigfr 0:9f7ee7ed13e4 419 if(abs(angularLeft)>abs(angularRight)) {
Ludwigfr 0:9f7ee7ed13e4 420 angularRight=this->speed*abs(angularRight/angularLeft)*this->sign1(angularRight);
Ludwigfr 0:9f7ee7ed13e4 421 angularLeft=this->speed*this->sign1(angularLeft);
Ludwigfr 0:9f7ee7ed13e4 422 }
Ludwigfr 0:9f7ee7ed13e4 423 else {
Ludwigfr 0:9f7ee7ed13e4 424 angularLeft=this->speed*abs(angularLeft/angularRight)*this->sign1(angularLeft);
Ludwigfr 0:9f7ee7ed13e4 425 angularRight=this->speed*this->sign1(angularRight);
Ludwigfr 0:9f7ee7ed13e4 426 }
Ludwigfr 0:9f7ee7ed13e4 427 leftMotor(this->sign2(angularLeft),abs(angularLeft));
Ludwigfr 0:9f7ee7ed13e4 428 rightMotor(this->sign2(angularRight),abs(angularRight));
Ludwigfr 0:9f7ee7ed13e4 429 }
Ludwigfr 0:9f7ee7ed13e4 430
Ludwigfr 0:9f7ee7ed13e4 431 void MiniExplorerCoimbra::update_force(int widthIndice, int heightIndice, float* forceRepulsionComputedX, float* forceRepulsionComputedY ){
Ludwigfr 0:9f7ee7ed13e4 432 //get the coordonate of the map and the robot in the ortonormal space
Ludwigfr 0:9f7ee7ed13e4 433 float xWorldCell=this->map.cell_width_coordinate_to_world(widthIndice);
Ludwigfr 0:9f7ee7ed13e4 434 float yWorldCell=this->map.cell_height_coordinate_to_world(heightIndice);
Ludwigfr 0:9f7ee7ed13e4 435 //compute the distance beetween the cell and the robot
Ludwigfr 0:9f7ee7ed13e4 436 float distanceCellToRobot=sqrt(pow(xWorldCell-this->xWorld,2)+pow(yWorldCell-this->yWorld,2));
Ludwigfr 0:9f7ee7ed13e4 437 //check if the cell is in range
Ludwigfr 0:9f7ee7ed13e4 438 if(distanceCellToRobot <= this->rangeForce) {
Ludwigfr 0:9f7ee7ed13e4 439 float probaCell=this->map.get_proba_cell(widthIndice,heightIndice);
Ludwigfr 0:9f7ee7ed13e4 440 *forceRepulsionComputedX+=this->repulsionConstantForce*probaCell*(xWorldCell-this->xWorld)/pow(distanceCellToRobot,3);
Ludwigfr 0:9f7ee7ed13e4 441 *forceRepulsionComputedY+=this->repulsionConstantForce*probaCell*(yWorldCell-this->yWorld)/pow(distanceCellToRobot,3);
Ludwigfr 0:9f7ee7ed13e4 442 }
Ludwigfr 0:9f7ee7ed13e4 443 }
Ludwigfr 0:9f7ee7ed13e4 444
Ludwigfr 0:9f7ee7ed13e4 445 //return 1 if positiv, -1 if negativ
Ludwigfr 0:9f7ee7ed13e4 446 float MiniExplorerCoimbra::sign1(float value){
Ludwigfr 0:9f7ee7ed13e4 447 if(value>=0)
Ludwigfr 0:9f7ee7ed13e4 448 return 1;
Ludwigfr 0:9f7ee7ed13e4 449 else
Ludwigfr 0:9f7ee7ed13e4 450 return -1;
Ludwigfr 0:9f7ee7ed13e4 451 }
Ludwigfr 0:9f7ee7ed13e4 452
Ludwigfr 0:9f7ee7ed13e4 453 //return 1 if positiv, 0 if negativ
Ludwigfr 0:9f7ee7ed13e4 454 int MiniExplorerCoimbra::sign2(float value){
Ludwigfr 0:9f7ee7ed13e4 455 if(value>=0)
Ludwigfr 0:9f7ee7ed13e4 456 return 1;
Ludwigfr 0:9f7ee7ed13e4 457 else
Ludwigfr 0:9f7ee7ed13e4 458 return 0;
Ludwigfr 0:9f7ee7ed13e4 459 }
Ludwigfr 0:9f7ee7ed13e4 460
Ludwigfr 0:9f7ee7ed13e4 461 /*
Ludwigfr 0:9f7ee7ed13e4 462 void MiniExplorerCoimbra::test_procedure_lab_4(){
Ludwigfr 0:9f7ee7ed13e4 463 this->map.fill_map_with_initial();
Ludwigfr 0:9f7ee7ed13e4 464 float xEstimatedK=this->xWorld;
Ludwigfr 0:9f7ee7ed13e4 465 float yEstimatedK=this->yWorld;
Ludwigfr 0:9f7ee7ed13e4 466 float thetaWorldEstimatedK = this->thetaWorld;
Ludwigfr 0:9f7ee7ed13e4 467 float distanceMoved;
Ludwigfr 0:9f7ee7ed13e4 468 float angleMoved;
Ludwigfr 0:9f7ee7ed13e4 469 float PreviousCovarianceOdometricPositionEstimate[3][3];
Ludwigfr 0:9f7ee7ed13e4 470 PreviousCovarianceOdometricPositionEstimate[0][0]=0;
Ludwigfr 0:9f7ee7ed13e4 471 PreviousCovarianceOdometricPositionEstimate[0][1]=0;
Ludwigfr 0:9f7ee7ed13e4 472 PreviousCovarianceOdometricPositionEstimate[0][2]=0;
Ludwigfr 0:9f7ee7ed13e4 473 PreviousCovarianceOdometricPositionEstimate[1][0]=0;
Ludwigfr 0:9f7ee7ed13e4 474 PreviousCovarianceOdometricPositionEstimate[1][1]=0;
Ludwigfr 0:9f7ee7ed13e4 475 PreviousCovarianceOdometricPositionEstimate[1][2]=0;
Ludwigfr 0:9f7ee7ed13e4 476 PreviousCovarianceOdometricPositionEstimate[2][0]=0;
Ludwigfr 0:9f7ee7ed13e4 477 PreviousCovarianceOdometricPositionEstimate[2][1]=0;
Ludwigfr 0:9f7ee7ed13e4 478 PreviousCovarianceOdometricPositionEstimate[2][2]=0;
Ludwigfr 0:9f7ee7ed13e4 479 while(1){
Ludwigfr 0:9f7ee7ed13e4 480 distanceMoved=50;
Ludwigfr 0:9f7ee7ed13e4 481 angleMoved=0;
Ludwigfr 0:9f7ee7ed13e4 482 this->go_straight_line(50);
Ludwigfr 0:9f7ee7ed13e4 483 wait(1);
Ludwigfr 0:9f7ee7ed13e4 484 procedure_lab_4(xEstimatedK,yEstimatedK,thetaWorldEstimatedK,distanceMoved,angleMoved,PreviousCovarianceOdometricPositionEstimate);
Ludwigfr 0:9f7ee7ed13e4 485 pc.printf("\n\r x:%f,y:%f,theta:%f",this->xWorld,this->yWorld,this->thetaWorld);
Ludwigfr 0:9f7ee7ed13e4 486 pc.printf("\n\r xEstim:%f,yEstim:%f,thetaEstim:%f",xEstimatedK,yEstimatedK,thetaWorldEstimatedK);
Ludwigfr 0:9f7ee7ed13e4 487 this->turn_to_target(PI/2);
Ludwigfr 0:9f7ee7ed13e4 488 distanceMoved=0;
Ludwigfr 0:9f7ee7ed13e4 489 angleMoved=PI/2;
Ludwigfr 0:9f7ee7ed13e4 490 procedure_lab_4(xEstimatedK,yEstimatedK,thetaWorldEstimatedK,distanceMoved,angleMoved,PreviousCovarianceOdometricPositionEstimate);
Ludwigfr 0:9f7ee7ed13e4 491 pc.printf("\n\r x:%f,y:%f,theta:%f",this->xWorld,this->yWorld,this->thetaWorld);
Ludwigfr 0:9f7ee7ed13e4 492 pc.printf("\n\r xEstim:%f,yEstim:%f,thetaEstim:%f",xEstimatedK,yEstimatedK,thetaWorldEstimatedK);
Ludwigfr 0:9f7ee7ed13e4 493 }
Ludwigfr 0:9f7ee7ed13e4 494 }
Ludwigfr 0:9f7ee7ed13e4 495
Ludwigfr 0:9f7ee7ed13e4 496 void MiniExplorerCoimbra::go_straight_line(float distanceCm){
Ludwigfr 0:9f7ee7ed13e4 497 leftMotor(1,1);
Ludwigfr 0:9f7ee7ed13e4 498 rightMotor(1,1);
Ludwigfr 0:9f7ee7ed13e4 499 float xEstimated=this->xWorld+distanceCm*cos(this->thetaWorld);
Ludwigfr 0:9f7ee7ed13e4 500 float yEstimated=this->yWorld+distanceCm*sin(this->thetaWorld);
Ludwigfr 0:9f7ee7ed13e4 501 while(1){
Ludwigfr 0:9f7ee7ed13e4 502 this->myOdometria();
Ludwigfr 0:9f7ee7ed13e4 503 if(abs(xEstimated-this->xWorld) < 0.1 && abs(yEstimated-this->yWorld) < 0.1)
Ludwigfr 0:9f7ee7ed13e4 504 break;
Ludwigfr 0:9f7ee7ed13e4 505 }
Ludwigfr 0:9f7ee7ed13e4 506 leftMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 507 rightMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 508 }
Ludwigfr 0:9f7ee7ed13e4 509
Ludwigfr 0:9f7ee7ed13e4 510 //compute predicted localisation (assume movements are either straight lines or pure rotation), update PreviousCovarianceOdometricPositionEstimate
Ludwigfr 0:9f7ee7ed13e4 511 //TODO tweak constant rewritte it good
Ludwigfr 0:9f7ee7ed13e4 512 void MiniExplorerCoimbra::procedure_lab_4(float xEstimatedK,float yEstimatedK, float thetaWorldEstimatedK, float distanceMoved, float angleMoved, float PreviousCovarianceOdometricPositionEstimate[3][3]){
Ludwigfr 0:9f7ee7ed13e4 513
Ludwigfr 0:9f7ee7ed13e4 514 float distanceMovedByLeftWheel=distanceMoved/2;
Ludwigfr 0:9f7ee7ed13e4 515 float distanceMovedByRightWheel=distanceMoved/2;
Ludwigfr 0:9f7ee7ed13e4 516 if(angleMoved !=0){
Ludwigfr 0:9f7ee7ed13e4 517 //TODO check that not sure
Ludwigfr 0:9f7ee7ed13e4 518 distanceMovedByLeftWheel=-angleMoved/(2*this->distanceWheels);
Ludwigfr 0:9f7ee7ed13e4 519 distanceMovedByRightWheel=angleMoved/(2*this->distanceWheels);
Ludwigfr 0:9f7ee7ed13e4 520 }else{
Ludwigfr 0:9f7ee7ed13e4 521 distanceMovedByLeftWheel=distanceMoved/2;
Ludwigfr 0:9f7ee7ed13e4 522 distanceMovedByRightWheel=distanceMoved/2;
Ludwigfr 0:9f7ee7ed13e4 523 }
Ludwigfr 0:9f7ee7ed13e4 524
Ludwigfr 0:9f7ee7ed13e4 525 float xEstimatedKNext=xEstimatedK+distanceMoved*cos(angleMoved);
Ludwigfr 0:9f7ee7ed13e4 526 float yEstimatedKNext=yEstimatedK+distanceMoved*sin(angleMoved);
Ludwigfr 0:9f7ee7ed13e4 527 float thetaWorldEstimatedKNext=thetaWorldEstimatedK+angleMoved;
Ludwigfr 0:9f7ee7ed13e4 528
Ludwigfr 0:9f7ee7ed13e4 529 float kRight=0.1;//error constant
Ludwigfr 0:9f7ee7ed13e4 530 float kLeft=0.1;//error constant
Ludwigfr 0:9f7ee7ed13e4 531 float motionIncrementCovarianceMatrix[2][2];
Ludwigfr 0:9f7ee7ed13e4 532 motionIncrementCovarianceMatrix[0][0]=kRight*abs(distanceMovedByRightWheel);
Ludwigfr 0:9f7ee7ed13e4 533 motionIncrementCovarianceMatrix[0][1]=0;
Ludwigfr 0:9f7ee7ed13e4 534 motionIncrementCovarianceMatrix[1][0]=0;
Ludwigfr 0:9f7ee7ed13e4 535 motionIncrementCovarianceMatrix[1][1]=kLeft*abs(distanceMovedByLeftWheel);
Ludwigfr 0:9f7ee7ed13e4 536
Ludwigfr 0:9f7ee7ed13e4 537 float jacobianFp[3][3];
Ludwigfr 0:9f7ee7ed13e4 538 jacobianFp[0][0]=1;
Ludwigfr 0:9f7ee7ed13e4 539 jacobianFp[0][1]=0;
Ludwigfr 0:9f7ee7ed13e4 540 jacobianFp[0][2]=-distanceMoved*sin(thetaWorldEstimatedK+angleMoved/2);
Ludwigfr 0:9f7ee7ed13e4 541 jacobianFp[1][0]=0;
Ludwigfr 0:9f7ee7ed13e4 542 jacobianFp[1][1]=1;
Ludwigfr 0:9f7ee7ed13e4 543 jacobianFp[1][2]=distanceMoved*cos(thetaWorldEstimatedK+angleMoved/2);;
Ludwigfr 0:9f7ee7ed13e4 544 jacobianFp[2][0]=0;
Ludwigfr 0:9f7ee7ed13e4 545 jacobianFp[2][1]=0;
Ludwigfr 0:9f7ee7ed13e4 546 jacobianFp[2][2]=1;
Ludwigfr 0:9f7ee7ed13e4 547
Ludwigfr 0:9f7ee7ed13e4 548 float jacobianFpTranspose[3][3];
Ludwigfr 0:9f7ee7ed13e4 549 jacobianFpTranspose[0][0]=1;
Ludwigfr 0:9f7ee7ed13e4 550 jacobianFpTranspose[0][1]=0;
Ludwigfr 0:9f7ee7ed13e4 551 jacobianFpTranspose[0][2]=0;
Ludwigfr 0:9f7ee7ed13e4 552 jacobianFpTranspose[1][0]=0;
Ludwigfr 0:9f7ee7ed13e4 553 jacobianFpTranspose[1][1]=1;
Ludwigfr 0:9f7ee7ed13e4 554 jacobianFpTranspose[1][2]=0;
Ludwigfr 0:9f7ee7ed13e4 555 jacobianFpTranspose[2][0]=-distanceMoved*sin(thetaWorldEstimatedK+angleMoved/2);
Ludwigfr 0:9f7ee7ed13e4 556 jacobianFpTranspose[2][1]=distanceMoved*cos(thetaWorldEstimatedK+angleMoved/2);
Ludwigfr 0:9f7ee7ed13e4 557 jacobianFpTranspose[2][2]=1;
Ludwigfr 0:9f7ee7ed13e4 558
Ludwigfr 0:9f7ee7ed13e4 559 float jacobianFDeltarl[3][2];
Ludwigfr 0:9f7ee7ed13e4 560 jacobianFDeltarl[0][0]=(1/2)*cos(thetaWorldEstimatedK+angleMoved/2)-(distanceMoved/(2*this->distanceWheels))*sin(thetaWorldEstimatedK+angleMoved/2);
Ludwigfr 0:9f7ee7ed13e4 561 jacobianFDeltarl[0][1]=(1/2)*cos(thetaWorldEstimatedK+angleMoved/2)+(distanceMoved/(2*this->distanceWheels))*sin(thetaWorldEstimatedK+angleMoved/2);
Ludwigfr 0:9f7ee7ed13e4 562 jacobianFDeltarl[1][0]=(1/2)*sin(thetaWorldEstimatedK+angleMoved/2)+(distanceMoved/(2*this->distanceWheels))*cos(thetaWorldEstimatedK+angleMoved/2);
Ludwigfr 0:9f7ee7ed13e4 563 jacobianFDeltarl[1][1]=(1/2)*sin(thetaWorldEstimatedK+angleMoved/2)-(distanceMoved/(2*this->distanceWheels))*cos(thetaWorldEstimatedK+angleMoved/2);
Ludwigfr 0:9f7ee7ed13e4 564 jacobianFDeltarl[2][0]=1/this->distanceWheels;
Ludwigfr 0:9f7ee7ed13e4 565 jacobianFDeltarl[2][1]=-1/this->distanceWheels;
Ludwigfr 0:9f7ee7ed13e4 566
Ludwigfr 0:9f7ee7ed13e4 567 float jacobianFDeltarlTranspose[2][3];//1,3,5;2,4,6
Ludwigfr 0:9f7ee7ed13e4 568 jacobianFDeltarlTranspose[0][0]=(1/2)*cos(thetaWorldEstimatedK+angleMoved/2)-(distanceMoved/(2*this->distanceWheels))*sin(thetaWorldEstimatedK+angleMoved/2);
Ludwigfr 0:9f7ee7ed13e4 569 jacobianFDeltarlTranspose[0][1]=(1/2)*sin(thetaWorldEstimatedK+angleMoved/2)+(distanceMoved/(2*this->distanceWheels))*cos(thetaWorldEstimatedK+angleMoved/2);
Ludwigfr 0:9f7ee7ed13e4 570 jacobianFDeltarlTranspose[0][2]=1/this->distanceWheels;
Ludwigfr 0:9f7ee7ed13e4 571 jacobianFDeltarlTranspose[1][0]=(1/2)*cos(thetaWorldEstimatedK+angleMoved/2)+(distanceMoved/(2*this->distanceWheels))*sin(thetaWorldEstimatedK+angleMoved/2);
Ludwigfr 0:9f7ee7ed13e4 572 jacobianFDeltarlTranspose[1][1]=(1/2)*sin(thetaWorldEstimatedK+angleMoved/2)-(distanceMoved/(2*this->distanceWheels))*cos(thetaWorldEstimatedK+angleMoved/2);
Ludwigfr 0:9f7ee7ed13e4 573 jacobianFDeltarlTranspose[1][2]=-1/this->distanceWheels;
Ludwigfr 0:9f7ee7ed13e4 574
Ludwigfr 0:9f7ee7ed13e4 575 float tempMatrix3_3[3][3];
Ludwigfr 0:9f7ee7ed13e4 576 int i;
Ludwigfr 0:9f7ee7ed13e4 577 int j;
Ludwigfr 0:9f7ee7ed13e4 578 int k;
Ludwigfr 0:9f7ee7ed13e4 579 for( i = 0; i < 3; ++i){//mult 3*3, 3*3
Ludwigfr 0:9f7ee7ed13e4 580 for(j = 0; j < 3; ++j){
Ludwigfr 0:9f7ee7ed13e4 581 for(k = 0; k < 3; ++k){
Ludwigfr 0:9f7ee7ed13e4 582 tempMatrix3_3[i][j] += jacobianFp[i][k] * PreviousCovarianceOdometricPositionEstimate[k][j];
Ludwigfr 0:9f7ee7ed13e4 583 }
Ludwigfr 0:9f7ee7ed13e4 584 }
Ludwigfr 0:9f7ee7ed13e4 585 }
Ludwigfr 0:9f7ee7ed13e4 586 float sndTempMatrix3_3[3][3];
Ludwigfr 0:9f7ee7ed13e4 587 for(i = 0; i < 3; ++i){//mult 3*3, 3*3
Ludwigfr 0:9f7ee7ed13e4 588 for(j = 0; j < 3; ++j){
Ludwigfr 0:9f7ee7ed13e4 589 for(k = 0; k < 3; ++k){
Ludwigfr 0:9f7ee7ed13e4 590 sndTempMatrix3_3[i][j] += tempMatrix3_3[i][k] * jacobianFpTranspose[k][j];
Ludwigfr 0:9f7ee7ed13e4 591 }
Ludwigfr 0:9f7ee7ed13e4 592 }
Ludwigfr 0:9f7ee7ed13e4 593 }
Ludwigfr 0:9f7ee7ed13e4 594 float tempMatrix3_2[3][2];
Ludwigfr 0:9f7ee7ed13e4 595 for(i = 0; i < 3; ++i){//mult 3*2 , 2,2
Ludwigfr 0:9f7ee7ed13e4 596 for(j = 0; j < 2; ++j){
Ludwigfr 0:9f7ee7ed13e4 597 for(k = 0; k < 2; ++k){
Ludwigfr 0:9f7ee7ed13e4 598 tempMatrix3_2[i][j] += jacobianFDeltarl[i][k] * motionIncrementCovarianceMatrix[k][j];
Ludwigfr 0:9f7ee7ed13e4 599 }
Ludwigfr 0:9f7ee7ed13e4 600 }
Ludwigfr 0:9f7ee7ed13e4 601 }
Ludwigfr 0:9f7ee7ed13e4 602 float thrdTempMatrix3_3[3][3];
Ludwigfr 0:9f7ee7ed13e4 603 for(i = 0; i < 3; ++i){//mult 3*2 , 2,3
Ludwigfr 0:9f7ee7ed13e4 604 for(j = 0; j < 3; ++j){
Ludwigfr 0:9f7ee7ed13e4 605 for(k = 0; k < 2; ++k){
Ludwigfr 0:9f7ee7ed13e4 606 thrdTempMatrix3_3[i][j] += tempMatrix3_2[i][k] * jacobianFDeltarlTranspose[k][j];
Ludwigfr 0:9f7ee7ed13e4 607 }
Ludwigfr 0:9f7ee7ed13e4 608 }
Ludwigfr 0:9f7ee7ed13e4 609 }
Ludwigfr 0:9f7ee7ed13e4 610 float newCovarianceOdometricPositionEstimate[3][3];
Ludwigfr 0:9f7ee7ed13e4 611 for(i = 0; i < 3; ++i){//add 3*3 , 3*3
Ludwigfr 0:9f7ee7ed13e4 612 for(j = 0; j < 3; ++j){
Ludwigfr 0:9f7ee7ed13e4 613 newCovarianceOdometricPositionEstimate[i][j]=sndTempMatrix3_3[i][j]+thrdTempMatrix3_3[i][j];
Ludwigfr 0:9f7ee7ed13e4 614 }
Ludwigfr 0:9f7ee7ed13e4 615 }
Ludwigfr 0:9f7ee7ed13e4 616
Ludwigfr 0:9f7ee7ed13e4 617 //check measurements from sonars, see if they passed the validation gate
Ludwigfr 0:9f7ee7ed13e4 618 float leftCm = get_distance_left_sensor()/10;
Ludwigfr 0:9f7ee7ed13e4 619 float frontCm = get_distance_front_sensor()/10;
Ludwigfr 0:9f7ee7ed13e4 620 float rightCm = get_distance_right_sensor()/10;
Ludwigfr 0:9f7ee7ed13e4 621 //if superior to sonar range, put the value to sonar max range + 1
Ludwigfr 0:9f7ee7ed13e4 622 if(leftCm > this->sonarLeft.maxRange)
Ludwigfr 0:9f7ee7ed13e4 623 leftCm=this->sonarLeft.maxRange;
Ludwigfr 0:9f7ee7ed13e4 624 if(frontCm > this->sonarFront.maxRange)
Ludwigfr 0:9f7ee7ed13e4 625 frontCm=this->sonarFront.maxRange;
Ludwigfr 0:9f7ee7ed13e4 626 if(rightCm > this->sonarRight.maxRange)
Ludwigfr 0:9f7ee7ed13e4 627 rightCm=this->sonarRight.maxRange;
Ludwigfr 0:9f7ee7ed13e4 628 //get the estimated measure we should have according to our knowledge of the map and the previously computed localisation
Ludwigfr 0:9f7ee7ed13e4 629 float leftCmEstimated=this->sonarLeft.maxRange;
Ludwigfr 0:9f7ee7ed13e4 630 float frontCmEstimated=this->sonarFront.maxRange;
Ludwigfr 0:9f7ee7ed13e4 631 float rightCmEstimated=this->sonarRight.maxRange;
Ludwigfr 0:9f7ee7ed13e4 632 float xWorldCell;
Ludwigfr 0:9f7ee7ed13e4 633 float yWorldCell;
Ludwigfr 0:9f7ee7ed13e4 634 float currDistance;
Ludwigfr 0:9f7ee7ed13e4 635 float xClosestCellLeft;
Ludwigfr 0:9f7ee7ed13e4 636 float yClosestCellLeft;
Ludwigfr 0:9f7ee7ed13e4 637 float xClosestCellFront;
Ludwigfr 0:9f7ee7ed13e4 638 float yClosestCellFront;
Ludwigfr 0:9f7ee7ed13e4 639 float xClosestCellRight;
Ludwigfr 0:9f7ee7ed13e4 640 float yClosestCellRight;
Ludwigfr 0:9f7ee7ed13e4 641 for(int i=0;i<this->map.nbCellWidth;i++){
Ludwigfr 0:9f7ee7ed13e4 642 for(int j=0;j<this->map.nbCellHeight;j++){
Ludwigfr 0:9f7ee7ed13e4 643 //check if occupied, if not discard
Ludwigfr 0:9f7ee7ed13e4 644 if(this->map.get_proba_cell(i,j)==1){
Ludwigfr 0:9f7ee7ed13e4 645 //check if in sonar range
Ludwigfr 0:9f7ee7ed13e4 646 xWorldCell=this->map.cell_width_coordinate_to_world(i);
Ludwigfr 0:9f7ee7ed13e4 647 yWorldCell=this->map.cell_height_coordinate_to_world(j);
Ludwigfr 0:9f7ee7ed13e4 648 //check left
Ludwigfr 0:9f7ee7ed13e4 649 currDistance=this->sonarLeft.isInRange(xWorldCell,yWorldCell,xEstimatedKNext,yEstimatedKNext,thetaWorldEstimatedKNext);
Ludwigfr 0:9f7ee7ed13e4 650 if((currDistance < this->sonarLeft.maxRange) && currDistance!=-1){
Ludwigfr 0:9f7ee7ed13e4 651 //check if distance is lower than previous, update lowest if so
Ludwigfr 0:9f7ee7ed13e4 652 if(currDistance < leftCmEstimated){
Ludwigfr 0:9f7ee7ed13e4 653 leftCmEstimated= currDistance;
Ludwigfr 0:9f7ee7ed13e4 654 xClosestCellLeft=xWorldCell;
Ludwigfr 0:9f7ee7ed13e4 655 yClosestCellLeft=yWorldCell;
Ludwigfr 0:9f7ee7ed13e4 656 }
Ludwigfr 0:9f7ee7ed13e4 657 }
Ludwigfr 0:9f7ee7ed13e4 658 //check front
Ludwigfr 0:9f7ee7ed13e4 659 currDistance=this->sonarFront.isInRange(xWorldCell,yWorldCell,xEstimatedKNext,yEstimatedKNext,thetaWorldEstimatedKNext);
Ludwigfr 0:9f7ee7ed13e4 660 if((currDistance < this->sonarFront.maxRange) && currDistance!=-1){
Ludwigfr 0:9f7ee7ed13e4 661 //check if distance is lower than previous, update lowest if so
Ludwigfr 0:9f7ee7ed13e4 662 if(currDistance < frontCmEstimated){
Ludwigfr 0:9f7ee7ed13e4 663 frontCmEstimated= currDistance;
Ludwigfr 0:9f7ee7ed13e4 664 xClosestCellFront=xWorldCell;
Ludwigfr 0:9f7ee7ed13e4 665 yClosestCellFront=yWorldCell;
Ludwigfr 0:9f7ee7ed13e4 666 }
Ludwigfr 0:9f7ee7ed13e4 667 }
Ludwigfr 0:9f7ee7ed13e4 668 //check right
Ludwigfr 0:9f7ee7ed13e4 669 currDistance=this->sonarRight.isInRange(xWorldCell,yWorldCell,xEstimatedKNext,yEstimatedKNext,thetaWorldEstimatedKNext);
Ludwigfr 0:9f7ee7ed13e4 670 if((currDistance < this->sonarRight.maxRange) && currDistance!=-1){
Ludwigfr 0:9f7ee7ed13e4 671 //check if distance is lower than previous, update lowest if so
Ludwigfr 0:9f7ee7ed13e4 672 if(currDistance < rightCmEstimated){
Ludwigfr 0:9f7ee7ed13e4 673 rightCmEstimated= currDistance;
Ludwigfr 0:9f7ee7ed13e4 674 xClosestCellRight=xWorldCell;
Ludwigfr 0:9f7ee7ed13e4 675 yClosestCellRight=yWorldCell;
Ludwigfr 0:9f7ee7ed13e4 676 }
Ludwigfr 0:9f7ee7ed13e4 677 }
Ludwigfr 0:9f7ee7ed13e4 678 }
Ludwigfr 0:9f7ee7ed13e4 679 }
Ludwigfr 0:9f7ee7ed13e4 680 }
Ludwigfr 0:9f7ee7ed13e4 681 //get the innoncation: the value of the difference between the actual measure and what we anticipated
Ludwigfr 0:9f7ee7ed13e4 682 float innovationLeft=leftCm-leftCmEstimated;
Ludwigfr 0:9f7ee7ed13e4 683 float innovationFront=frontCm-frontCmEstimated;
Ludwigfr 0:9f7ee7ed13e4 684 float innovationRight=-rightCm-rightCmEstimated;
Ludwigfr 0:9f7ee7ed13e4 685 //compute jacobian of observation
Ludwigfr 0:9f7ee7ed13e4 686 float jacobianOfObservationLeft[3];
Ludwigfr 0:9f7ee7ed13e4 687 float jacobianOfObservationRight[3];
Ludwigfr 0:9f7ee7ed13e4 688 float jacobianOfObservationFront[3];
Ludwigfr 0:9f7ee7ed13e4 689 float xSonarLeft=xEstimatedKNext+this->sonarLeft.distanceX;
Ludwigfr 0:9f7ee7ed13e4 690 float ySonarLeft=yEstimatedKNext+this->sonarLeft.distanceY;
Ludwigfr 0:9f7ee7ed13e4 691 //left
Ludwigfr 0:9f7ee7ed13e4 692 jacobianOfObservationLeft[0]=(xSonarLeft-xClosestCellLeft)/leftCmEstimated;
Ludwigfr 0:9f7ee7ed13e4 693 jacobianOfObservationLeft[1]=(ySonarLeft-yClosestCellLeft)/leftCmEstimated;
Ludwigfr 0:9f7ee7ed13e4 694 jacobianOfObservationLeft[2]=(xClosestCellLeft-xSonarLeft)*(xSonarLeft*sin(thetaWorldEstimatedKNext)+ySonarLeft*cos(thetaWorldEstimatedKNext))+(yClosestCellLeft-ySonarLeft)*(-xSonarLeft*cos(thetaWorldEstimatedKNext)+ySonarLeft*sin(thetaWorldEstimatedKNext));
Ludwigfr 0:9f7ee7ed13e4 695 //front
Ludwigfr 0:9f7ee7ed13e4 696 float xSonarFront=xEstimatedKNext+this->sonarFront.distanceX;
Ludwigfr 0:9f7ee7ed13e4 697 float ySonarFront=yEstimatedKNext+this->sonarFront.distanceY;
Ludwigfr 0:9f7ee7ed13e4 698 jacobianOfObservationFront[0]=(xSonarFront-xClosestCellFront)/frontCmEstimated;
Ludwigfr 0:9f7ee7ed13e4 699 jacobianOfObservationFront[1]=(ySonarFront-yClosestCellFront)/frontCmEstimated;
Ludwigfr 0:9f7ee7ed13e4 700 jacobianOfObservationFront[2]=(xClosestCellFront-xSonarFront)*(xSonarFront*sin(thetaWorldEstimatedKNext)+ySonarFront*cos(thetaWorldEstimatedKNext))+(yClosestCellFront-ySonarFront)*(-xSonarFront*cos(thetaWorldEstimatedKNext)+ySonarFront*sin(thetaWorldEstimatedKNext));
Ludwigfr 0:9f7ee7ed13e4 701 //right
Ludwigfr 0:9f7ee7ed13e4 702 float xSonarRight=xEstimatedKNext+this->sonarRight.distanceX;
Ludwigfr 0:9f7ee7ed13e4 703 float ySonarRight=yEstimatedKNext+this->sonarRight.distanceY;
Ludwigfr 0:9f7ee7ed13e4 704 jacobianOfObservationRight[0]=(xSonarRight-xClosestCellRight)/rightCmEstimated;
Ludwigfr 0:9f7ee7ed13e4 705 jacobianOfObservationRight[1]=(ySonarRight-yClosestCellRight)/rightCmEstimated;
Ludwigfr 0:9f7ee7ed13e4 706 jacobianOfObservationRight[2]=(xClosestCellRight-xSonarRight)*(xSonarRight*sin(thetaWorldEstimatedKNext)+ySonarRight*cos(thetaWorldEstimatedKNext))+(yClosestCellRight-ySonarRight)*(-xSonarRight*cos(thetaWorldEstimatedKNext)+ySonarRight*sin(thetaWorldEstimatedKNext));
Ludwigfr 0:9f7ee7ed13e4 707
Ludwigfr 0:9f7ee7ed13e4 708 //left
Ludwigfr 0:9f7ee7ed13e4 709 float TempMatrix3_3InnovationLeft[3];
Ludwigfr 0:9f7ee7ed13e4 710 TempMatrix3_3InnovationLeft[0]=jacobianOfObservationLeft[0]*newCovarianceOdometricPositionEstimate[0][0]+jacobianOfObservationLeft[1]*newCovarianceOdometricPositionEstimate[1][0]+jacobianOfObservationLeft[2]*newCovarianceOdometricPositionEstimate[2][0];
Ludwigfr 0:9f7ee7ed13e4 711 TempMatrix3_3InnovationLeft[1]=jacobianOfObservationLeft[0]*newCovarianceOdometricPositionEstimate[0][1]+jacobianOfObservationLeft[1]*newCovarianceOdometricPositionEstimate[1][1]+jacobianOfObservationLeft[2]*newCovarianceOdometricPositionEstimate[2][1];
Ludwigfr 0:9f7ee7ed13e4 712 TempMatrix3_3InnovationLeft[2]=jacobianOfObservationLeft[0]*newCovarianceOdometricPositionEstimate[0][2]+jacobianOfObservationLeft[1]*newCovarianceOdometricPositionEstimate[1][2]+jacobianOfObservationLeft[2]*newCovarianceOdometricPositionEstimate[2][2];
Ludwigfr 0:9f7ee7ed13e4 713 float innovationConvarianceLeft=TempMatrix3_3InnovationLeft[0]*jacobianOfObservationLeft[0]+TempMatrix3_3InnovationLeft[1]*jacobianOfObservationLeft[1]+TempMatrix3_3InnovationLeft[2]*jacobianOfObservationLeft[2];
Ludwigfr 0:9f7ee7ed13e4 714 //front
Ludwigfr 0:9f7ee7ed13e4 715 float TempMatrix3_3InnovationFront[3];
Ludwigfr 0:9f7ee7ed13e4 716 TempMatrix3_3InnovationFront[0]=jacobianOfObservationFront[0]*newCovarianceOdometricPositionEstimate[0][0]+jacobianOfObservationFront[1]*newCovarianceOdometricPositionEstimate[1][0]+jacobianOfObservationFront[2]*newCovarianceOdometricPositionEstimate[2][0];
Ludwigfr 0:9f7ee7ed13e4 717 TempMatrix3_3InnovationFront[1]=jacobianOfObservationFront[0]*newCovarianceOdometricPositionEstimate[0][1]+jacobianOfObservationFront[1]*newCovarianceOdometricPositionEstimate[1][1]+jacobianOfObservationFront[2]*newCovarianceOdometricPositionEstimate[2][1];
Ludwigfr 0:9f7ee7ed13e4 718 TempMatrix3_3InnovationFront[2]=jacobianOfObservationFront[0]*newCovarianceOdometricPositionEstimate[0][2]+jacobianOfObservationFront[1]*newCovarianceOdometricPositionEstimate[1][2]+jacobianOfObservationFront[2]*newCovarianceOdometricPositionEstimate[2][2];
Ludwigfr 0:9f7ee7ed13e4 719 float innovationConvarianceFront=TempMatrix3_3InnovationFront[0]*jacobianOfObservationFront[0]+TempMatrix3_3InnovationFront[1]*jacobianOfObservationFront[1]+TempMatrix3_3InnovationFront[2]*jacobianOfObservationFront[2];
Ludwigfr 0:9f7ee7ed13e4 720 //right
Ludwigfr 0:9f7ee7ed13e4 721 float TempMatrix3_3InnovationRight[3];
Ludwigfr 0:9f7ee7ed13e4 722 TempMatrix3_3InnovationRight[0]=jacobianOfObservationRight[0]*newCovarianceOdometricPositionEstimate[0][0]+jacobianOfObservationRight[1]*newCovarianceOdometricPositionEstimate[1][0]+jacobianOfObservationRight[2]*newCovarianceOdometricPositionEstimate[2][0];
Ludwigfr 0:9f7ee7ed13e4 723 TempMatrix3_3InnovationRight[1]=jacobianOfObservationRight[0]*newCovarianceOdometricPositionEstimate[0][1]+jacobianOfObservationRight[1]*newCovarianceOdometricPositionEstimate[1][1]+jacobianOfObservationRight[2]*newCovarianceOdometricPositionEstimate[2][1];
Ludwigfr 0:9f7ee7ed13e4 724 TempMatrix3_3InnovationRight[2]=jacobianOfObservationRight[0]*newCovarianceOdometricPositionEstimate[0][2]+jacobianOfObservationRight[1]*newCovarianceOdometricPositionEstimate[1][2]+jacobianOfObservationRight[2]*newCovarianceOdometricPositionEstimate[2][2];
Ludwigfr 0:9f7ee7ed13e4 725 float innovationConvarianceRight=TempMatrix3_3InnovationRight[0]*jacobianOfObservationRight[0]+TempMatrix3_3InnovationRight[1]*jacobianOfObservationRight[1]+TempMatrix3_3InnovationRight[2]*jacobianOfObservationRight[2];
Ludwigfr 0:9f7ee7ed13e4 726
Ludwigfr 0:9f7ee7ed13e4 727 //check if it pass the validation gate
Ludwigfr 0:9f7ee7ed13e4 728 float gateScoreLeft=innovationLeft*innovationLeft/innovationConvarianceLeft;
Ludwigfr 0:9f7ee7ed13e4 729 float gateScoreFront=innovationFront*innovationFront/innovationConvarianceFront;
Ludwigfr 0:9f7ee7ed13e4 730 float gateScoreRight=innovationRight*innovationRight/innovationConvarianceRight;
Ludwigfr 0:9f7ee7ed13e4 731 int leftPassed=0;
Ludwigfr 0:9f7ee7ed13e4 732 int frontPassed=0;
Ludwigfr 0:9f7ee7ed13e4 733 int rightPassed=0;
Ludwigfr 0:9f7ee7ed13e4 734 int e=10;//constant
Ludwigfr 0:9f7ee7ed13e4 735 if(gateScoreLeft < e)
Ludwigfr 0:9f7ee7ed13e4 736 leftPassed=1;
Ludwigfr 0:9f7ee7ed13e4 737 if(gateScoreFront < e)
Ludwigfr 0:9f7ee7ed13e4 738 frontPassed=10;
Ludwigfr 0:9f7ee7ed13e4 739 if(gateScoreRight < e)
Ludwigfr 0:9f7ee7ed13e4 740 rightPassed=100;
Ludwigfr 0:9f7ee7ed13e4 741 //for those who passed
Ludwigfr 0:9f7ee7ed13e4 742 //compute composite innovation
Ludwigfr 0:9f7ee7ed13e4 743 float compositeInnovation[3];
Ludwigfr 0:9f7ee7ed13e4 744 int nbPassed=leftPassed+frontPassed+rightPassed;
Ludwigfr 0:9f7ee7ed13e4 745 switch(nbPassed){
Ludwigfr 0:9f7ee7ed13e4 746 case 0://none
Ludwigfr 0:9f7ee7ed13e4 747 compositeInnovation[0]=0;
Ludwigfr 0:9f7ee7ed13e4 748 compositeInnovation[1]=0;
Ludwigfr 0:9f7ee7ed13e4 749 compositeInnovation[2]=0;
Ludwigfr 0:9f7ee7ed13e4 750 break;
Ludwigfr 0:9f7ee7ed13e4 751 case 1://left
Ludwigfr 0:9f7ee7ed13e4 752 compositeInnovation[0]=innovationLeft;
Ludwigfr 0:9f7ee7ed13e4 753 compositeInnovation[1]=0;
Ludwigfr 0:9f7ee7ed13e4 754 compositeInnovation[2]=0;
Ludwigfr 0:9f7ee7ed13e4 755 break;
Ludwigfr 0:9f7ee7ed13e4 756 case 10://front
Ludwigfr 0:9f7ee7ed13e4 757 compositeInnovation[0]=0;
Ludwigfr 0:9f7ee7ed13e4 758 compositeInnovation[1]=innovationFront;
Ludwigfr 0:9f7ee7ed13e4 759 compositeInnovation[2]=0;
Ludwigfr 0:9f7ee7ed13e4 760 break;
Ludwigfr 0:9f7ee7ed13e4 761 case 11://left and front
Ludwigfr 0:9f7ee7ed13e4 762 compositeInnovation[0]=innovationLeft;
Ludwigfr 0:9f7ee7ed13e4 763 compositeInnovation[1]=innovationFront;
Ludwigfr 0:9f7ee7ed13e4 764 compositeInnovation[2]=0;
Ludwigfr 0:9f7ee7ed13e4 765 break;
Ludwigfr 0:9f7ee7ed13e4 766 case 100://right
Ludwigfr 0:9f7ee7ed13e4 767 compositeInnovation[0]=0;
Ludwigfr 0:9f7ee7ed13e4 768 compositeInnovation[1]=0;
Ludwigfr 0:9f7ee7ed13e4 769 compositeInnovation[2]=innovationRight;
Ludwigfr 0:9f7ee7ed13e4 770 break;
Ludwigfr 0:9f7ee7ed13e4 771 case 101://right and left
Ludwigfr 0:9f7ee7ed13e4 772 compositeInnovation[0]=innovationLeft;
Ludwigfr 0:9f7ee7ed13e4 773 compositeInnovation[1]=0;
Ludwigfr 0:9f7ee7ed13e4 774 compositeInnovation[2]=innovationRight;
Ludwigfr 0:9f7ee7ed13e4 775 break;
Ludwigfr 0:9f7ee7ed13e4 776 case 110://right and front
Ludwigfr 0:9f7ee7ed13e4 777 compositeInnovation[0]=0;
Ludwigfr 0:9f7ee7ed13e4 778 compositeInnovation[1]=innovationFront;
Ludwigfr 0:9f7ee7ed13e4 779 compositeInnovation[2]=innovationRight;
Ludwigfr 0:9f7ee7ed13e4 780 break;
Ludwigfr 0:9f7ee7ed13e4 781 case 111://right front and left
Ludwigfr 0:9f7ee7ed13e4 782 compositeInnovation[0]=innovationLeft;
Ludwigfr 0:9f7ee7ed13e4 783 compositeInnovation[1]=innovationFront;
Ludwigfr 0:9f7ee7ed13e4 784 compositeInnovation[2]=innovationRight;
Ludwigfr 0:9f7ee7ed13e4 785 break;
Ludwigfr 0:9f7ee7ed13e4 786 }
Ludwigfr 0:9f7ee7ed13e4 787 //compute composite measurement Jacobian
Ludwigfr 0:9f7ee7ed13e4 788 float *compositeMeasurementJacobian;
Ludwigfr 0:9f7ee7ed13e4 789 switch(nbPassed){
Ludwigfr 0:9f7ee7ed13e4 790 case 0://none
Ludwigfr 0:9f7ee7ed13e4 791 break;
Ludwigfr 0:9f7ee7ed13e4 792 case 1://left
Ludwigfr 0:9f7ee7ed13e4 793 //compositeMeasurementJacobian = jacobianOfObservationLeft
Ludwigfr 0:9f7ee7ed13e4 794 break;
Ludwigfr 0:9f7ee7ed13e4 795 case 10://front
Ludwigfr 0:9f7ee7ed13e4 796 //compositeMeasurementJacobian = jacobianOfObservationFront
Ludwigfr 0:9f7ee7ed13e4 797 break;
Ludwigfr 0:9f7ee7ed13e4 798 case 11://left and front
Ludwigfr 0:9f7ee7ed13e4 799 compositeMeasurementJacobian=new float[6];
Ludwigfr 0:9f7ee7ed13e4 800 compositeMeasurementJacobian[0]= jacobianOfObservationLeft[0];
Ludwigfr 0:9f7ee7ed13e4 801 compositeMeasurementJacobian[1]= jacobianOfObservationLeft[1];
Ludwigfr 0:9f7ee7ed13e4 802 compositeMeasurementJacobian[2]= jacobianOfObservationLeft[2];
Ludwigfr 0:9f7ee7ed13e4 803 compositeMeasurementJacobian[3]= jacobianOfObservationFront[0];
Ludwigfr 0:9f7ee7ed13e4 804 compositeMeasurementJacobian[4]= jacobianOfObservationFront[1];
Ludwigfr 0:9f7ee7ed13e4 805 compositeMeasurementJacobian[5]= jacobianOfObservationFront[2];
Ludwigfr 0:9f7ee7ed13e4 806 break;
Ludwigfr 0:9f7ee7ed13e4 807 case 100://right
Ludwigfr 0:9f7ee7ed13e4 808 //compositeMeasurementJacobian = jacobianOfObservationRight
Ludwigfr 0:9f7ee7ed13e4 809 break;
Ludwigfr 0:9f7ee7ed13e4 810 case 101://right and left
Ludwigfr 0:9f7ee7ed13e4 811 compositeMeasurementJacobian=new float[6];
Ludwigfr 0:9f7ee7ed13e4 812 compositeMeasurementJacobian[0]= jacobianOfObservationLeft[0];
Ludwigfr 0:9f7ee7ed13e4 813 compositeMeasurementJacobian[1]= jacobianOfObservationLeft[1];
Ludwigfr 0:9f7ee7ed13e4 814 compositeMeasurementJacobian[2]= jacobianOfObservationLeft[2];
Ludwigfr 0:9f7ee7ed13e4 815 compositeMeasurementJacobian[3]= jacobianOfObservationRight[0];
Ludwigfr 0:9f7ee7ed13e4 816 compositeMeasurementJacobian[4]= jacobianOfObservationRight[1];
Ludwigfr 0:9f7ee7ed13e4 817 compositeMeasurementJacobian[5]= jacobianOfObservationRight[2];
Ludwigfr 0:9f7ee7ed13e4 818
Ludwigfr 0:9f7ee7ed13e4 819 break;
Ludwigfr 0:9f7ee7ed13e4 820 case 110://right and front
Ludwigfr 0:9f7ee7ed13e4 821 compositeMeasurementJacobian=new float[6];
Ludwigfr 0:9f7ee7ed13e4 822 compositeMeasurementJacobian[0]= jacobianOfObservationFront[0];
Ludwigfr 0:9f7ee7ed13e4 823 compositeMeasurementJacobian[1]= jacobianOfObservationFront[1];
Ludwigfr 0:9f7ee7ed13e4 824 compositeMeasurementJacobian[2]= jacobianOfObservationFront[2];
Ludwigfr 0:9f7ee7ed13e4 825 compositeMeasurementJacobian[3]= jacobianOfObservationRight[0];
Ludwigfr 0:9f7ee7ed13e4 826 compositeMeasurementJacobian[4]= jacobianOfObservationRight[1];
Ludwigfr 0:9f7ee7ed13e4 827 compositeMeasurementJacobian[5]= jacobianOfObservationRight[2];
Ludwigfr 0:9f7ee7ed13e4 828
Ludwigfr 0:9f7ee7ed13e4 829 break;
Ludwigfr 0:9f7ee7ed13e4 830 case 111://right front and left
Ludwigfr 0:9f7ee7ed13e4 831 compositeMeasurementJacobian=new float[9];
Ludwigfr 0:9f7ee7ed13e4 832 compositeMeasurementJacobian[0]= jacobianOfObservationLeft[0];
Ludwigfr 0:9f7ee7ed13e4 833 compositeMeasurementJacobian[1]= jacobianOfObservationLeft[1];
Ludwigfr 0:9f7ee7ed13e4 834 compositeMeasurementJacobian[2]= jacobianOfObservationLeft[2];
Ludwigfr 0:9f7ee7ed13e4 835 compositeMeasurementJacobian[3]= jacobianOfObservationFront[0];
Ludwigfr 0:9f7ee7ed13e4 836 compositeMeasurementJacobian[4]= jacobianOfObservationFront[1];
Ludwigfr 0:9f7ee7ed13e4 837 compositeMeasurementJacobian[5]= jacobianOfObservationFront[2];
Ludwigfr 0:9f7ee7ed13e4 838 compositeMeasurementJacobian[6]= jacobianOfObservationRight[0];
Ludwigfr 0:9f7ee7ed13e4 839 compositeMeasurementJacobian[7]= jacobianOfObservationRight[1];
Ludwigfr 0:9f7ee7ed13e4 840 compositeMeasurementJacobian[8]= jacobianOfObservationRight[2];
Ludwigfr 0:9f7ee7ed13e4 841 break;
Ludwigfr 0:9f7ee7ed13e4 842 }
Ludwigfr 0:9f7ee7ed13e4 843 //compute composite innovation covariance TODO dont have time, I assume you can just multiply them, can also simply all this easily
Ludwigfr 0:9f7ee7ed13e4 844 float compositeInnovationCovariance;
Ludwigfr 0:9f7ee7ed13e4 845 switch(nbPassed){
Ludwigfr 0:9f7ee7ed13e4 846 case 0://none
Ludwigfr 0:9f7ee7ed13e4 847 compositeInnovationCovariance=1;
Ludwigfr 0:9f7ee7ed13e4 848 break;
Ludwigfr 0:9f7ee7ed13e4 849 case 1://left
Ludwigfr 0:9f7ee7ed13e4 850 compositeInnovationCovariance = innovationConvarianceLeft;
Ludwigfr 0:9f7ee7ed13e4 851
Ludwigfr 0:9f7ee7ed13e4 852 break;
Ludwigfr 0:9f7ee7ed13e4 853 case 10://front
Ludwigfr 0:9f7ee7ed13e4 854 compositeInnovationCovariance = innovationConvarianceFront;
Ludwigfr 0:9f7ee7ed13e4 855 break;
Ludwigfr 0:9f7ee7ed13e4 856 case 11://left and front
Ludwigfr 0:9f7ee7ed13e4 857 compositeInnovationCovariance=innovationConvarianceLeft*innovationConvarianceFront;
Ludwigfr 0:9f7ee7ed13e4 858
Ludwigfr 0:9f7ee7ed13e4 859 break;
Ludwigfr 0:9f7ee7ed13e4 860 case 100://right
Ludwigfr 0:9f7ee7ed13e4 861 compositeInnovationCovariance = innovationConvarianceRight;
Ludwigfr 0:9f7ee7ed13e4 862 break;
Ludwigfr 0:9f7ee7ed13e4 863 case 101://right and left
Ludwigfr 0:9f7ee7ed13e4 864 compositeInnovationCovariance=innovationConvarianceLeft*innovationConvarianceRight;
Ludwigfr 0:9f7ee7ed13e4 865 break;
Ludwigfr 0:9f7ee7ed13e4 866 case 110://right and front
Ludwigfr 0:9f7ee7ed13e4 867 compositeInnovationCovariance=innovationConvarianceFront*innovationConvarianceRight;
Ludwigfr 0:9f7ee7ed13e4 868
Ludwigfr 0:9f7ee7ed13e4 869
Ludwigfr 0:9f7ee7ed13e4 870 break;
Ludwigfr 0:9f7ee7ed13e4 871 case 111://right front and left
Ludwigfr 0:9f7ee7ed13e4 872 compositeInnovationCovariance=innovationConvarianceLeft*innovationConvarianceFront*innovationConvarianceRight;
Ludwigfr 0:9f7ee7ed13e4 873
Ludwigfr 0:9f7ee7ed13e4 874 break;
Ludwigfr 0:9f7ee7ed13e4 875 }
Ludwigfr 0:9f7ee7ed13e4 876
Ludwigfr 0:9f7ee7ed13e4 877 //compute kalman gain
Ludwigfr 0:9f7ee7ed13e4 878 float kalmanGain[3][3];
Ludwigfr 0:9f7ee7ed13e4 879 switch(nbPassed){
Ludwigfr 0:9f7ee7ed13e4 880 //mult nbSonarPassed*3 , 3*3
Ludwigfr 0:9f7ee7ed13e4 881 case 0://none
Ludwigfr 0:9f7ee7ed13e4 882 break;
Ludwigfr 0:9f7ee7ed13e4 883 case 1://left
Ludwigfr 0:9f7ee7ed13e4 884
Ludwigfr 0:9f7ee7ed13e4 885 for(i = 0; i < 3; ++i){//mult 3*3, 3*1
Ludwigfr 0:9f7ee7ed13e4 886 for(j = 0; j < 1; ++j){
Ludwigfr 0:9f7ee7ed13e4 887 for(k = 0; k < 3; ++k){
Ludwigfr 0:9f7ee7ed13e4 888 kalmanGain[i][j] += newCovarianceOdometricPositionEstimate[i][k] * compositeInnovationCovariance[k];
Ludwigfr 0:9f7ee7ed13e4 889 }
Ludwigfr 0:9f7ee7ed13e4 890 }
Ludwigfr 0:9f7ee7ed13e4 891 }
Ludwigfr 0:9f7ee7ed13e4 892
Ludwigfr 0:9f7ee7ed13e4 893 break;
Ludwigfr 0:9f7ee7ed13e4 894 case 10://front
Ludwigfr 0:9f7ee7ed13e4 895 for(i = 0; i < 3; ++i){//mult 3*3, 3*1
Ludwigfr 0:9f7ee7ed13e4 896 for(j = 0; j < 1; ++j){
Ludwigfr 0:9f7ee7ed13e4 897 for(k = 0; k < 3; ++k){
Ludwigfr 0:9f7ee7ed13e4 898 kalmanGain[i][j] += newCovarianceOdometricPositionEstimate[i][k] * compositeInnovationCovariance[k];
Ludwigfr 0:9f7ee7ed13e4 899 }
Ludwigfr 0:9f7ee7ed13e4 900 }
Ludwigfr 0:9f7ee7ed13e4 901 }
Ludwigfr 0:9f7ee7ed13e4 902 break;
Ludwigfr 0:9f7ee7ed13e4 903 case 11://left and front
Ludwigfr 0:9f7ee7ed13e4 904 for(i = 0; i < 3; ++i){//mult 3*3, 3*2
Ludwigfr 0:9f7ee7ed13e4 905 for(j = 0; j < 2; ++j){
Ludwigfr 0:9f7ee7ed13e4 906 for(k = 0; k < 3; ++k){
Ludwigfr 0:9f7ee7ed13e4 907 kalmanGain[i][j] += newCovarianceOdometricPositionEstimate[i][k] * compositeInnovationCovariance[k+j*3];
Ludwigfr 0:9f7ee7ed13e4 908 }
Ludwigfr 0:9f7ee7ed13e4 909 }
Ludwigfr 0:9f7ee7ed13e4 910 }
Ludwigfr 0:9f7ee7ed13e4 911
Ludwigfr 0:9f7ee7ed13e4 912 break;
Ludwigfr 0:9f7ee7ed13e4 913 case 100://right
Ludwigfr 0:9f7ee7ed13e4 914 for(i = 0; i < 3; ++i){//mult 3*3, 3*1
Ludwigfr 0:9f7ee7ed13e4 915 for(j = 0; j < 1; ++j){
Ludwigfr 0:9f7ee7ed13e4 916 for(k = 0; k < 3; ++k){
Ludwigfr 0:9f7ee7ed13e4 917 kalmanGain[i][j] += newCovarianceOdometricPositionEstimate[i][k] * compositeInnovationCovariance[k];
Ludwigfr 0:9f7ee7ed13e4 918 }
Ludwigfr 0:9f7ee7ed13e4 919 }
Ludwigfr 0:9f7ee7ed13e4 920 }
Ludwigfr 0:9f7ee7ed13e4 921 break;
Ludwigfr 0:9f7ee7ed13e4 922 case 101://right and left
Ludwigfr 0:9f7ee7ed13e4 923 for(i = 0; i < 3; ++i){//mult 3*3, 3*2
Ludwigfr 0:9f7ee7ed13e4 924 for(j = 0; j < 2; ++j){
Ludwigfr 0:9f7ee7ed13e4 925 for(k = 0; k < 3; ++k){
Ludwigfr 0:9f7ee7ed13e4 926 kalmanGain[i][j] += newCovarianceOdometricPositionEstimate[i][k] * compositeInnovationCovariance[k+j*3];
Ludwigfr 0:9f7ee7ed13e4 927 }
Ludwigfr 0:9f7ee7ed13e4 928 }
Ludwigfr 0:9f7ee7ed13e4 929 }
Ludwigfr 0:9f7ee7ed13e4 930
Ludwigfr 0:9f7ee7ed13e4 931 break;
Ludwigfr 0:9f7ee7ed13e4 932 case 110://right and front
Ludwigfr 0:9f7ee7ed13e4 933 for(i = 0; i < 3; ++i){//mult 3*3, 3*2
Ludwigfr 0:9f7ee7ed13e4 934 for(j = 0; j < 2; ++j){
Ludwigfr 0:9f7ee7ed13e4 935 for(k = 0; k < 3; ++k){
Ludwigfr 0:9f7ee7ed13e4 936 kalmanGain[i][j] += newCovarianceOdometricPositionEstimate[i][k] * compositeInnovationCovariance[k+j*3];
Ludwigfr 0:9f7ee7ed13e4 937 }
Ludwigfr 0:9f7ee7ed13e4 938 }
Ludwigfr 0:9f7ee7ed13e4 939 }
Ludwigfr 0:9f7ee7ed13e4 940
Ludwigfr 0:9f7ee7ed13e4 941
Ludwigfr 0:9f7ee7ed13e4 942 break;
Ludwigfr 0:9f7ee7ed13e4 943 case 111://right front and left
Ludwigfr 0:9f7ee7ed13e4 944 for(i = 0; i < 3; ++i){//mult 3*3, 3*3
Ludwigfr 0:9f7ee7ed13e4 945 for(j = 0; j < 3; ++j){
Ludwigfr 0:9f7ee7ed13e4 946 for(k = 0; k < 3; ++k){
Ludwigfr 0:9f7ee7ed13e4 947 kalmanGain[i][j] += newCovarianceOdometricPositionEstimate[i][k] * compositeInnovationCovariance[k+j*3];
Ludwigfr 0:9f7ee7ed13e4 948 }
Ludwigfr 0:9f7ee7ed13e4 949 }
Ludwigfr 0:9f7ee7ed13e4 950 }
Ludwigfr 0:9f7ee7ed13e4 951
Ludwigfr 0:9f7ee7ed13e4 952 break;
Ludwigfr 0:9f7ee7ed13e4 953 }
Ludwigfr 0:9f7ee7ed13e4 954
Ludwigfr 0:9f7ee7ed13e4 955 for(i = 0; i < 3; ++i){//mult 3*3, 3*3
Ludwigfr 0:9f7ee7ed13e4 956 for(j = 0; j < 3; ++j){
Ludwigfr 0:9f7ee7ed13e4 957 kalmanGain[i][j] = kalmanGain[i][j]/compositeInnovationCovariance;
Ludwigfr 0:9f7ee7ed13e4 958 }
Ludwigfr 0:9f7ee7ed13e4 959 }
Ludwigfr 0:9f7ee7ed13e4 960 //compute kalman gain * composite innovation
Ludwigfr 0:9f7ee7ed13e4 961 //mult 3*3 , 3*1
Ludwigfr 0:9f7ee7ed13e4 962 float result3_1[3];
Ludwigfr 0:9f7ee7ed13e4 963 for(i = 0; i < 3; ++i){//mult 3*3, 3*1
Ludwigfr 0:9f7ee7ed13e4 964 for(k = 0; k < 3; ++k){
Ludwigfr 0:9f7ee7ed13e4 965 result3_1[i] += kalmanGain[i][k] * compositeInnovation[k];
Ludwigfr 0:9f7ee7ed13e4 966 }
Ludwigfr 0:9f7ee7ed13e4 967 }
Ludwigfr 0:9f7ee7ed13e4 968 //compute updated robot position estimate
Ludwigfr 0:9f7ee7ed13e4 969 float xEstimatedKLast=xEstimatedKNext+result3_1[0];
Ludwigfr 0:9f7ee7ed13e4 970 float yEstimatedKLast=yEstimatedKNext+result3_1[1];
Ludwigfr 0:9f7ee7ed13e4 971 float thetaWorldEstimatedKLast=thetaWorldEstimatedKNext+result3_1[2];
Ludwigfr 0:9f7ee7ed13e4 972 //store the resultant covariance for next estimation
Ludwigfr 0:9f7ee7ed13e4 973
Ludwigfr 0:9f7ee7ed13e4 974 float kalmanGainTranspose[3][3];
Ludwigfr 0:9f7ee7ed13e4 975 kalmanGainTranspose[0][0]=kalmanGain[0][0];
Ludwigfr 0:9f7ee7ed13e4 976 kalmanGainTranspose[0][1]=kalmanGain[1][0];
Ludwigfr 0:9f7ee7ed13e4 977 kalmanGainTranspose[0][2]=kalmanGain[2][0];
Ludwigfr 0:9f7ee7ed13e4 978 kalmanGainTranspose[1][0]=kalmanGain[0][1];
Ludwigfr 0:9f7ee7ed13e4 979 kalmanGainTranspose[1][1]=kalmanGain[1][1];
Ludwigfr 0:9f7ee7ed13e4 980 kalmanGainTranspose[1][2]=kalmanGain[2][1];
Ludwigfr 0:9f7ee7ed13e4 981 kalmanGainTranspose[2][0]=kalmanGain[0][2];
Ludwigfr 0:9f7ee7ed13e4 982 kalmanGainTranspose[2][1]=kalmanGain[1][2];
Ludwigfr 0:9f7ee7ed13e4 983 kalmanGainTranspose[2][2]=kalmanGain[2][2];
Ludwigfr 0:9f7ee7ed13e4 984
Ludwigfr 0:9f7ee7ed13e4 985 //mult 3*3 , 3*3
Ludwigfr 0:9f7ee7ed13e4 986 float fithTempMatrix3_3[3][3];
Ludwigfr 0:9f7ee7ed13e4 987 for(i = 0; i < 3; ++i){//mult 3*3 , 3*3
Ludwigfr 0:9f7ee7ed13e4 988 for(j = 0; j < 3; ++j){
Ludwigfr 0:9f7ee7ed13e4 989 for(k = 0; k < 3; ++k){
Ludwigfr 0:9f7ee7ed13e4 990 fithTempMatrix3_3[i][j] += kalmanGain[i][k] * kalmanGainTranspose[k][j];
Ludwigfr 0:9f7ee7ed13e4 991 }
Ludwigfr 0:9f7ee7ed13e4 992 }
Ludwigfr 0:9f7ee7ed13e4 993 }
Ludwigfr 0:9f7ee7ed13e4 994 for(i = 0; i < 3; ++i){//add 3*3 , 3*3
Ludwigfr 0:9f7ee7ed13e4 995 for(j = 0; j < 3; ++j){
Ludwigfr 0:9f7ee7ed13e4 996 fithTempMatrix3_3[i][j]=fithTempMatrix3_3[i][j]*compositeInnovationCovariance;
Ludwigfr 0:9f7ee7ed13e4 997 }
Ludwigfr 0:9f7ee7ed13e4 998 }
Ludwigfr 0:9f7ee7ed13e4 999 float covariancePositionEsimatedLast[3][3];
Ludwigfr 0:9f7ee7ed13e4 1000 for(i = 0; i < 3; ++i){//add 3*3 , 3*3
Ludwigfr 0:9f7ee7ed13e4 1001 for(j = 0; j < 3; ++j){
Ludwigfr 0:9f7ee7ed13e4 1002 covariancePositionEsimatedLast[i][j]=fithTempMatrix3_3[i][j]+newCovarianceOdometricPositionEstimate[i][j];
Ludwigfr 0:9f7ee7ed13e4 1003 }
Ludwigfr 0:9f7ee7ed13e4 1004 }
Ludwigfr 0:9f7ee7ed13e4 1005 //update PreviousCovarianceOdometricPositionEstimate
Ludwigfr 0:9f7ee7ed13e4 1006 for(i = 0; i < 3; ++i){
Ludwigfr 0:9f7ee7ed13e4 1007 for(j = 0; j < 3; ++j){
Ludwigfr 0:9f7ee7ed13e4 1008 PreviousCovarianceOdometricPositionEstimate[i][j]=covariancePositionEsimatedLast[i][j];
Ludwigfr 0:9f7ee7ed13e4 1009 }
Ludwigfr 0:9f7ee7ed13e4 1010 }
Ludwigfr 0:9f7ee7ed13e4 1011
Ludwigfr 0:9f7ee7ed13e4 1012 xEstimatedK=xEstimatedKLast;
Ludwigfr 0:9f7ee7ed13e4 1013 yEstimatedK=yEstimatedKLast;
Ludwigfr 0:9f7ee7ed13e4 1014 thetaWorldEstimatedK=thetaWorldEstimatedKLast;
Ludwigfr 0:9f7ee7ed13e4 1015 }
Ludwigfr 0:9f7ee7ed13e4 1016 */