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@6:0e8db3a23486, 2017-07-07 (annotated)

Committer:
Ludwigfr
Date:
Fri Jul 07 11:49:30 2017 +0000
Revision:
6:0e8db3a23486
Parent:
5:19f24c363418
Child:
8:072a76960e27
lol;

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