with class

Dependencies:   ISR_Mini-explorer mbed

Fork of VirtualForces by Georgios Tsamis

Committer:
Ludwigfr
Date:
Fri Jun 16 10:40:53 2017 +0000
Revision:
39:890439b495e3
Parent:
38:5ed7c79fb724
last version with the 4th lab;

Who changed what in which revision?

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