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