Yo Fleyt / Mbed 2 deprecated roboticLab_withclass_3_July

test morning

Dependencies:   ISR_Mini-explorer mbed

Fork of roboticLab_withclass_3_July by Georgios Tsamis

MiniExplorerCoimbra.cpp@8:072a76960e27, 2017-07-10 (annotated)

Committer:
Ludwigfr
Date:
Mon Jul 10 12:49:07 2017 +0000
Revision:
8:072a76960e27
Parent:
6:0e8db3a23486
Child:
9:1cc27f33d3e1
just in case of rollback

Who changed what in which revision?

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