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@5:19f24c363418, 2017-07-06 (annotated)

Committer:
Ludwigfr
Date:
Thu Jul 06 16:51:40 2017 +0000
Revision:
5:19f24c363418
Parent:
3:37345c109dfc
Child:
6:0e8db3a23486
latest 6/7 17h51

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 5:19f24c363418 819 this->OdometriaKalmanFilter(1,1);
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 5:19f24c363418 845 this->OdometriaKalmanFilter(1,1);
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 5:19f24c363418 876 this->OdometriaKalmanFilter(1,1);
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 5:19f24c363418 886 } while(distanceToTarget>1 || (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 5:19f24c363418 894 void MiniExplorerCoimbra::OdometriaKalmanFilter(float encoderRightFailureRate,float encoderLeftFailureRate){
Ludwigfr 5:19f24c363418 895 //=====KINEMATICS===========================
Ludwigfr 5:19f24c363418 896 float R_cm;
Ludwigfr 5:19f24c363418 897 float L_cm;
Ludwigfr 5:19f24c363418 898
Ludwigfr 5:19f24c363418 899 //fill R_cm and L_cm with how much is wheel has moved (custom robot.h)
Ludwigfr 5:19f24c363418 900 OdometriaKalman(&R_cm, &L_cm);
Ludwigfr 5:19f24c363418 901
Ludwigfr 5:19f24c363418 902 encoderRightFailureRate=0.95;
Ludwigfr 5:19f24c363418 903 encoderLeftFailureRate=1;
Ludwigfr 5:19f24c363418 904
Ludwigfr 5:19f24c363418 905 R_cm=R_cm*encoderRightFailureRate;
Ludwigfr 5:19f24c363418 906 L_cm=L_cm*encoderLeftFailureRate;
Ludwigfr 5:19f24c363418 907
Ludwigfr 5:19f24c363418 908 float distanceMoved=(R_cm+L_cm)/2;
Ludwigfr 5:19f24c363418 909 float angleMoved=(R_cm-L_cm)/this->distanceWheels;
Ludwigfr 5:19f24c363418 910
Ludwigfr 5:19f24c363418 911 float distanceMovedX=distanceMoved*cos(this->thetaWorld+angleMoved/2);
Ludwigfr 5:19f24c363418 912 float distanceMovedY=distanceMoved*sin(this->thetaWorld+angleMoved/2);
Ludwigfr 5:19f24c363418 913
Ludwigfr 5:19f24c363418 914 //try with world coordinate system
Ludwigfr 5:19f24c363418 915
Ludwigfr 5:19f24c363418 916 float xEstimatedK=this->xWorld+distanceMovedX;
Ludwigfr 5:19f24c363418 917 float yEstimatedK=this->yWorld+distanceMovedY;
Ludwigfr 5:19f24c363418 918 float thetaWorldEstimatedK = this->thetaWorld+angleMoved;
Ludwigfr 5:19f24c363418 919
Ludwigfr 5:19f24c363418 920 //try with robot coordinate system
Ludwigfr 5:19f24c363418 921 /*
Ludwigfr 5:19f24c363418 922 float xEstimatedK=X;
Ludwigfr 5:19f24c363418 923 float yEstimatedK=Y;
Ludwigfr 5:19f24c363418 924 float thetaWorldEstimatedK = theta;
Ludwigfr 5:19f24c363418 925 */
Ludwigfr 5:19f24c363418 926 //=====ERROR_MODEL===========================
Ludwigfr 5:19f24c363418 927
Ludwigfr 5:19f24c363418 928 //FP Matrix
Ludwigfr 5:19f24c363418 929 float Fp[3][3]={{1,0,0},{0,1,0},{0,0,1}};
Ludwigfr 5:19f24c363418 930
Ludwigfr 5:19f24c363418 931 Fp[0][2]=-1*distanceMoved*sin(this->thetaWorld+(angleMoved/2));
Ludwigfr 5:19f24c363418 932 Fp[1][2]=distanceMoved*cos(this->thetaWorld+(angleMoved/2));
Ludwigfr 5:19f24c363418 933
Ludwigfr 5:19f24c363418 934 //Frl matrix
Ludwigfr 5:19f24c363418 935 float Frl[3][2]={{0,0},{0,0},{(1/this->distanceWheels),-(1/this->distanceWheels)}};
Ludwigfr 5:19f24c363418 936
Ludwigfr 5:19f24c363418 937 Frl[0][0]=0.5*cos(this->thetaWorld+(angleMoved/2))-(distanceMoved/(2*this->distanceWheels))*sin(this->thetaWorld+(angleMoved/2));
Ludwigfr 5:19f24c363418 938 Frl[0][1]=0.5*cos(this->thetaWorld+(angleMoved/2))+(distanceMoved/(2*this->distanceWheels))*sin(this->thetaWorld+(angleMoved/2));
Ludwigfr 5:19f24c363418 939 Frl[1][0]=0.5*sin(this->thetaWorld+(angleMoved/2))+(distanceMoved/(2*this->distanceWheels))*cos(this->thetaWorld+(angleMoved/2));
Ludwigfr 5:19f24c363418 940 Frl[1][1]=0.5*sin(this->thetaWorld+(angleMoved/2))-(distanceMoved/(2*this->distanceWheels))*cos(this->thetaWorld+(angleMoved/2));
Ludwigfr 5:19f24c363418 941
Ludwigfr 5:19f24c363418 942 //error constants...
Ludwigfr 5:19f24c363418 943 float kr=1;
Ludwigfr 5:19f24c363418 944 float kl=1;
Ludwigfr 5:19f24c363418 945 float covar[2][2]={{kr*abs(R_cm), 0}, {0, kl*abs(L_cm)}};
Ludwigfr 5:19f24c363418 946
Ludwigfr 5:19f24c363418 947 //uncertainty positionx, and theta at
Ludwigfr 5:19f24c363418 948 //1,1,1
Ludwigfr 5:19f24c363418 949 float Q[3][3]={{1,0,0}, {0,2,0}, {0,0,0.01}};
Ludwigfr 5:19f24c363418 950
Ludwigfr 5:19f24c363418 951 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 952 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 953 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 954 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 955 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 956 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 957 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 958 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 959 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 960
Ludwigfr 5:19f24c363418 961 //=====OBSERVATION=====
Ludwigfr 5:19f24c363418 962 //get the estimated measure we should have according to our knowledge of the map and the previously computed localisation
Ludwigfr 5:19f24c363418 963
Ludwigfr 5:19f24c363418 964 float leftCmEstimated=this->sonarLeft.maxRange;
Ludwigfr 5:19f24c363418 965 float frontCmEstimated=this->sonarFront.maxRange;
Ludwigfr 5:19f24c363418 966 float rightCmEstimated=this->sonarRight.maxRange;
Ludwigfr 5:19f24c363418 967 float xWorldCell;
Ludwigfr 5:19f24c363418 968 float yWorldCell;
Ludwigfr 5:19f24c363418 969 float currDistance;
Ludwigfr 5:19f24c363418 970 float xClosestCellLeft;
Ludwigfr 5:19f24c363418 971 float yClosestCellLeft;
Ludwigfr 5:19f24c363418 972 float xClosestCellFront;
Ludwigfr 5:19f24c363418 973 float yClosestCellFront;
Ludwigfr 5:19f24c363418 974 float xClosestCellRight;
Ludwigfr 5:19f24c363418 975 float yClosestCellRight;
Ludwigfr 5:19f24c363418 976 //get theorical distance to sonar
Ludwigfr 5:19f24c363418 977 for(int i=0;i<this->map.nbCellWidth;i++){
Ludwigfr 5:19f24c363418 978 for(int j=0;j<this->map.nbCellHeight;j++){
Ludwigfr 5:19f24c363418 979 //check if occupied, if not discard
Ludwigfr 5:19f24c363418 980 if(this->map.get_proba_cell(i,j)<0.5){
Ludwigfr 5:19f24c363418 981 //check if in sonar range
Ludwigfr 5:19f24c363418 982 xWorldCell=this->map.cell_width_coordinate_to_world(i);
Ludwigfr 5:19f24c363418 983 yWorldCell=this->map.cell_height_coordinate_to_world(j);
Ludwigfr 5:19f24c363418 984 //check left
Ludwigfr 5:19f24c363418 985 currDistance=this->sonarLeft.isInRange(xWorldCell,yWorldCell,xEstimatedK,yEstimatedK,thetaWorldEstimatedK);
Ludwigfr 5:19f24c363418 986 if((currDistance < this->sonarLeft.maxRange) && currDistance!=-1){
Ludwigfr 5:19f24c363418 987 //check if distance is lower than previous, update lowest if so
Ludwigfr 5:19f24c363418 988 if(currDistance < leftCmEstimated){
Ludwigfr 5:19f24c363418 989 leftCmEstimated= currDistance;
Ludwigfr 5:19f24c363418 990 xClosestCellLeft=xWorldCell;
Ludwigfr 5:19f24c363418 991 yClosestCellLeft=yWorldCell;
Ludwigfr 5:19f24c363418 992 }
Ludwigfr 5:19f24c363418 993 }
Ludwigfr 5:19f24c363418 994 //check front
Ludwigfr 5:19f24c363418 995 currDistance=this->sonarFront.isInRange(xWorldCell,yWorldCell,xEstimatedK,yEstimatedK,thetaWorldEstimatedK);
Ludwigfr 5:19f24c363418 996 if((currDistance < this->sonarFront.maxRange) && currDistance!=-1){
Ludwigfr 5:19f24c363418 997 //check if distance is lower than previous, update lowest if so
Ludwigfr 5:19f24c363418 998 if(currDistance < frontCmEstimated){
Ludwigfr 5:19f24c363418 999 frontCmEstimated= currDistance;
Ludwigfr 5:19f24c363418 1000 xClosestCellFront=xWorldCell;
Ludwigfr 5:19f24c363418 1001 yClosestCellFront=yWorldCell;
Ludwigfr 5:19f24c363418 1002 }
Ludwigfr 5:19f24c363418 1003 }
Ludwigfr 5:19f24c363418 1004 //check right
Ludwigfr 5:19f24c363418 1005 currDistance=this->sonarRight.isInRange(xWorldCell,yWorldCell,xEstimatedK,yEstimatedK,thetaWorldEstimatedK);
Ludwigfr 5:19f24c363418 1006 if((currDistance < this->sonarRight.maxRange) && currDistance!=-1){
Ludwigfr 5:19f24c363418 1007 //check if distance is lower than previous, update lowest if so
Ludwigfr 5:19f24c363418 1008 if(currDistance < rightCmEstimated){
Ludwigfr 5:19f24c363418 1009 rightCmEstimated= currDistance;
Ludwigfr 5:19f24c363418 1010 xClosestCellRight=xWorldCell;
Ludwigfr 5:19f24c363418 1011 yClosestCellRight=yWorldCell;
Ludwigfr 5:19f24c363418 1012 }
Ludwigfr 5:19f24c363418 1013 }
Ludwigfr 5:19f24c363418 1014 }
Ludwigfr 5:19f24c363418 1015 }
Ludwigfr 5:19f24c363418 1016 }
Ludwigfr 5:19f24c363418 1017
Ludwigfr 5:19f24c363418 1018 //check measurements from sonars, see if they passed the validation gate
Ludwigfr 5:19f24c363418 1019 float leftCm = get_distance_left_sensor()/10;
Ludwigfr 5:19f24c363418 1020 float frontCm = get_distance_front_sensor()/10;
Ludwigfr 5:19f24c363418 1021 float rightCm = get_distance_right_sensor()/10;
Ludwigfr 5:19f24c363418 1022 //if superior to sonar range, put the value to sonar max range + 1
Ludwigfr 5:19f24c363418 1023 if(leftCm > this->sonarLeft.maxRange)
Ludwigfr 5:19f24c363418 1024 leftCm=this->sonarLeft.maxRange;
Ludwigfr 5:19f24c363418 1025 if(frontCm > this->sonarFront.maxRange)
Ludwigfr 5:19f24c363418 1026 frontCm=this->sonarFront.maxRange;
Ludwigfr 5:19f24c363418 1027 if(rightCm > this->sonarRight.maxRange)
Ludwigfr 5:19f24c363418 1028 rightCm=this->sonarRight.maxRange;
Ludwigfr 5:19f24c363418 1029
Ludwigfr 5:19f24c363418 1030 //======INNOVATION========
Ludwigfr 5:19f24c363418 1031 //get the innoncation: the value of the difference between the actual measure and what we anticipated
Ludwigfr 5:19f24c363418 1032 float innovationLeft=leftCm-leftCmEstimated;
Ludwigfr 5:19f24c363418 1033 float innovationFront=frontCm-frontCmEstimated;
Ludwigfr 5:19f24c363418 1034 float innovationRight=-rightCm-rightCmEstimated;
Ludwigfr 5:19f24c363418 1035 //compute jacobian of observation
Ludwigfr 5:19f24c363418 1036 float jacobianOfObservationLeft[1][3];
Ludwigfr 5:19f24c363418 1037 float jacobianOfObservationRight[1][3];
Ludwigfr 5:19f24c363418 1038 float jacobianOfObservationFront[1][3];
Ludwigfr 5:19f24c363418 1039 float xSonarLeft=xEstimatedK+this->sonarLeft.distanceX;
Ludwigfr 5:19f24c363418 1040 float ySonarLeft=yEstimatedK+this->sonarLeft.distanceY;
Ludwigfr 5:19f24c363418 1041 //left
Ludwigfr 5:19f24c363418 1042 jacobianOfObservationLeft[0][0]=(xSonarLeft-xClosestCellLeft)/leftCmEstimated;
Ludwigfr 5:19f24c363418 1043 jacobianOfObservationLeft[0][1]=(ySonarLeft-yClosestCellLeft)/leftCmEstimated;
Ludwigfr 5:19f24c363418 1044 jacobianOfObservationLeft[0][2]=(xClosestCellLeft-xSonarLeft)*(xSonarLeft*sin(thetaWorldEstimatedK)+ySonarLeft*cos(thetaWorldEstimatedK))+(yClosestCellLeft-ySonarLeft)*(-xSonarLeft*cos(thetaWorldEstimatedK)+ySonarLeft*sin(thetaWorldEstimatedK));
Ludwigfr 5:19f24c363418 1045 //front
Ludwigfr 5:19f24c363418 1046 float xSonarFront=xEstimatedK+this->sonarFront.distanceX;
Ludwigfr 5:19f24c363418 1047 float ySonarFront=yEstimatedK+this->sonarFront.distanceY;
Ludwigfr 5:19f24c363418 1048 jacobianOfObservationFront[0][0]=(xSonarFront-xClosestCellFront)/frontCmEstimated;
Ludwigfr 5:19f24c363418 1049 jacobianOfObservationFront[0][1]=(ySonarFront-yClosestCellFront)/frontCmEstimated;
Ludwigfr 5:19f24c363418 1050 jacobianOfObservationFront[0][2]=(xClosestCellFront-xSonarFront)*(xSonarFront*sin(thetaWorldEstimatedK)+ySonarFront*cos(thetaWorldEstimatedK))+(yClosestCellFront-ySonarFront)*(-xSonarFront*cos(thetaWorldEstimatedK)+ySonarFront*sin(thetaWorldEstimatedK));
Ludwigfr 5:19f24c363418 1051 //right
Ludwigfr 5:19f24c363418 1052 float xSonarRight=xEstimatedK+this->sonarRight.distanceX;
Ludwigfr 5:19f24c363418 1053 float ySonarRight=yEstimatedK+this->sonarRight.distanceY;
Ludwigfr 5:19f24c363418 1054 jacobianOfObservationRight[0][0]=(xSonarRight-xClosestCellRight)/rightCmEstimated;
Ludwigfr 5:19f24c363418 1055 jacobianOfObservationRight[0][1]=(ySonarRight-yClosestCellRight)/rightCmEstimated;
Ludwigfr 5:19f24c363418 1056 jacobianOfObservationRight[0][2]=(xClosestCellRight-xSonarRight)*(xSonarRight*sin(thetaWorldEstimatedK)+ySonarRight*cos(thetaWorldEstimatedK))+(yClosestCellRight-ySonarRight)*(-xSonarRight*cos(thetaWorldEstimatedK)+ySonarRight*sin(thetaWorldEstimatedK));
Ludwigfr 5:19f24c363418 1057
Ludwigfr 5:19f24c363418 1058 //error constants 0,0,0 sonars perfect; must be found by experimenting; gives mean and standanrt deviation
Ludwigfr 5:19f24c363418 1059 //let's assume
Ludwigfr 5:19f24c363418 1060 //in centimeters
Ludwigfr 5:19f24c363418 1061 float R_front=4;
Ludwigfr 5:19f24c363418 1062 float R_left=4;
Ludwigfr 5:19f24c363418 1063 float R_right=4;
Ludwigfr 5:19f24c363418 1064
Ludwigfr 5:19f24c363418 1065 //R-> 4 in diagonal
Ludwigfr 5:19f24c363418 1066
Ludwigfr 5:19f24c363418 1067 //S for each sonar (concatenated covariance matrix of innovation)
Ludwigfr 5:19f24c363418 1068 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 1069 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 1070 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 1071
Ludwigfr 5:19f24c363418 1072 //check if it pass the validation gate
Ludwigfr 5:19f24c363418 1073 float gateScoreLeft=innovationLeft*innovationLeft/innovationCovarianceLeft;
Ludwigfr 5:19f24c363418 1074 float gateScoreFront=innovationFront*innovationFront/innovationCovarianceFront;
Ludwigfr 5:19f24c363418 1075 float gateScoreRight=innovationRight*innovationRight/innovationCovarianceRight;
Ludwigfr 5:19f24c363418 1076 int leftPassed=0;
Ludwigfr 5:19f24c363418 1077 int frontPassed=0;
Ludwigfr 5:19f24c363418 1078 int rightPassed=0;
Ludwigfr 5:19f24c363418 1079
Ludwigfr 5:19f24c363418 1080 //5cm -> 25
Ludwigfr 5:19f24c363418 1081 int errorsquare=25;//constant error
Ludwigfr 5:19f24c363418 1082
Ludwigfr 5:19f24c363418 1083 if(gateScoreLeft <= errorsquare)
Ludwigfr 5:19f24c363418 1084 leftPassed=1;
Ludwigfr 5:19f24c363418 1085 if(gateScoreFront <= errorsquare)
Ludwigfr 5:19f24c363418 1086 frontPassed=10;
Ludwigfr 5:19f24c363418 1087 if(gateScoreRight <= errorsquare)
Ludwigfr 5:19f24c363418 1088 rightPassed=100;
Ludwigfr 5:19f24c363418 1089 //for those who passed
Ludwigfr 5:19f24c363418 1090 //compute composite innovation
Ludwigfr 5:19f24c363418 1091 int nbPassed=leftPassed+frontPassed+rightPassed;
Ludwigfr 5:19f24c363418 1092
Ludwigfr 5:19f24c363418 1093 float xEstimatedKNext=xEstimatedK;
Ludwigfr 5:19f24c363418 1094 float yEstimatedKNext=xEstimatedK;
Ludwigfr 5:19f24c363418 1095 float thetaWorldEstimatedKNext=thetaWorldEstimatedK;
Ludwigfr 5:19f24c363418 1096
Ludwigfr 5:19f24c363418 1097 float compositeInnovationCovariance3x3[3][3]={{0,0,0}, {0,0,0}, {0,0,0}};
Ludwigfr 5:19f24c363418 1098
Ludwigfr 5:19f24c363418 1099 float compositeInnovationCovariance2x2[2][2]={{0,0}, {0,0}};
Ludwigfr 5:19f24c363418 1100
Ludwigfr 5:19f24c363418 1101 float compositeInnovationCovariance1x1=0;
Ludwigfr 5:19f24c363418 1102
Ludwigfr 5:19f24c363418 1103 float kalmanGain3X1[3][1]={{0}, {0}, {0}};
Ludwigfr 5:19f24c363418 1104 float kalmanGain3X2[3][2]={{0,0}, {0.0}, {0,0}};
Ludwigfr 5:19f24c363418 1105 float kalmanGain3X3[3][3]={{0,0,0}, {0,0,0}, {0,0,0}};
Ludwigfr 5:19f24c363418 1106
Ludwigfr 5:19f24c363418 1107 //we do not use the composite measurement jacobian (16), we directly use the values from the measurement jacobian (jacobianOfObservation)
Ludwigfr 5:19f24c363418 1108
Ludwigfr 5:19f24c363418 1109 switch(nbPassed){
Ludwigfr 5:19f24c363418 1110 case 0://none
Ludwigfr 5:19f24c363418 1111 //nothings happens it's okay
Ludwigfr 5:19f24c363418 1112 break;
Ludwigfr 5:19f24c363418 1113 case 1://left
Ludwigfr 5:19f24c363418 1114 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 1115
Ludwigfr 5:19f24c363418 1116 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 1117 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 1118 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 1119
Ludwigfr 5:19f24c363418 1120 xEstimatedKNext+= kalmanGain3X1[0][0]*innovationRight;
Ludwigfr 5:19f24c363418 1121 yEstimatedKNext+= kalmanGain3X1[1][0]*innovationRight;
Ludwigfr 5:19f24c363418 1122 thetaWorldEstimatedKNext+= kalmanGain3X1[2][0]*innovationRight;
Ludwigfr 5:19f24c363418 1123
Ludwigfr 5:19f24c363418 1124 covariancePositionEstimationK[0][0]=- compositeInnovationCovariance1x1*pow(kalmanGain3X1[0][0],2) + covariancePositionEstimationK[0][0];
Ludwigfr 5:19f24c363418 1125 covariancePositionEstimationK[0][1]=covariancePositionEstimationK[0][1] - kalmanGain3X1[0][0]*kalmanGain3X1[1][0]*compositeInnovationCovariance1x1;
Ludwigfr 5:19f24c363418 1126 covariancePositionEstimationK[0][2]=covariancePositionEstimationK[0][2] - kalmanGain3X1[0][0]*kalmanGain3X1[2][0]*compositeInnovationCovariance1x1;
Ludwigfr 5:19f24c363418 1127 covariancePositionEstimationK[1][0]=covariancePositionEstimationK[1][0] - kalmanGain3X1[0][0]*kalmanGain3X1[1][0]*compositeInnovationCovariance1x1;
Ludwigfr 5:19f24c363418 1128 covariancePositionEstimationK[1][1]=- compositeInnovationCovariance1x1*pow(kalmanGain3X1[1][0],2) + covariancePositionEstimationK[1][1];
Ludwigfr 5:19f24c363418 1129 covariancePositionEstimationK[1][2]=covariancePositionEstimationK[1][2] - kalmanGain3X1[1][0]*kalmanGain3X1[2][0]*compositeInnovationCovariance1x1;
Ludwigfr 5:19f24c363418 1130 covariancePositionEstimationK[2][0]=covariancePositionEstimationK[2][0] - kalmanGain3X1[0][0]*kalmanGain3X1[2][0]*compositeInnovationCovariance1x1;
Ludwigfr 5:19f24c363418 1131 covariancePositionEstimationK[2][1]=covariancePositionEstimationK[2][1] - kalmanGain3X1[1][0]*kalmanGain3X1[2][0]*compositeInnovationCovariance1x1;
Ludwigfr 5:19f24c363418 1132 covariancePositionEstimationK[2][2]=- compositeInnovationCovariance1x1*pow(kalmanGain3X1[2][0],2) + covariancePositionEstimationK[2][2];
Ludwigfr 5:19f24c363418 1133
Ludwigfr 5:19f24c363418 1134 break;
Ludwigfr 5:19f24c363418 1135 case 10://front
Ludwigfr 5:19f24c363418 1136
Ludwigfr 5:19f24c363418 1137 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 1138
Ludwigfr 5:19f24c363418 1139 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 1140 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 1141 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 1142
Ludwigfr 5:19f24c363418 1143 xEstimatedKNext+= kalmanGain3X1[0][0]*innovationFront;
Ludwigfr 5:19f24c363418 1144 yEstimatedKNext+= kalmanGain3X1[1][0]*innovationFront;
Ludwigfr 5:19f24c363418 1145 thetaWorldEstimatedKNext+= kalmanGain3X1[2][0]*innovationFront;
Ludwigfr 5:19f24c363418 1146
Ludwigfr 5:19f24c363418 1147 covariancePositionEstimationK[0][0]=- compositeInnovationCovariance1x1*pow(kalmanGain3X1[0][0],2) + covariancePositionEstimationK[0][0];
Ludwigfr 5:19f24c363418 1148 covariancePositionEstimationK[0][1]=covariancePositionEstimationK[0][1] - kalmanGain3X1[0][0]*kalmanGain3X1[1][0]*compositeInnovationCovariance1x1;
Ludwigfr 5:19f24c363418 1149 covariancePositionEstimationK[0][2]=covariancePositionEstimationK[0][2] - kalmanGain3X1[0][0]*kalmanGain3X1[2][0]*compositeInnovationCovariance1x1;
Ludwigfr 5:19f24c363418 1150 covariancePositionEstimationK[1][0]=covariancePositionEstimationK[1][0] - kalmanGain3X1[0][0]*kalmanGain3X1[1][0]*compositeInnovationCovariance1x1;
Ludwigfr 5:19f24c363418 1151 covariancePositionEstimationK[1][1]=- compositeInnovationCovariance1x1*pow(kalmanGain3X1[1][0],2) + covariancePositionEstimationK[1][1];
Ludwigfr 5:19f24c363418 1152 covariancePositionEstimationK[1][2]=covariancePositionEstimationK[1][2] - kalmanGain3X1[1][0]*kalmanGain3X1[2][0]*compositeInnovationCovariance1x1;
Ludwigfr 5:19f24c363418 1153 covariancePositionEstimationK[2][0]=covariancePositionEstimationK[2][0] - kalmanGain3X1[0][0]*kalmanGain3X1[2][0]*compositeInnovationCovariance1x1;
Ludwigfr 5:19f24c363418 1154 covariancePositionEstimationK[2][1]=covariancePositionEstimationK[2][1] - kalmanGain3X1[1][0]*kalmanGain3X1[2][0]*compositeInnovationCovariance1x1;
Ludwigfr 5:19f24c363418 1155 covariancePositionEstimationK[2][2]=- compositeInnovationCovariance1x1*pow(kalmanGain3X1[2][0],2) + covariancePositionEstimationK[2][2];
Ludwigfr 5:19f24c363418 1156
Ludwigfr 5:19f24c363418 1157 break;
Ludwigfr 5:19f24c363418 1158 case 11://left and front
Ludwigfr 5:19f24c363418 1159 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 1160 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 1161 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 1162 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 1163
Ludwigfr 5:19f24c363418 1164 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 1165 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 1166 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 1167 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 1168 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 1169 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 1170
Ludwigfr 5:19f24c363418 1171 xEstimatedKNext+= kalmanGain3X2[0][0]*innovationFront + kalmanGain3X2[0][1]*innovationLeft;
Ludwigfr 5:19f24c363418 1172 yEstimatedKNext+= kalmanGain3X2[1][0]*innovationFront + kalmanGain3X2[1][1]*innovationLeft;
Ludwigfr 5:19f24c363418 1173 thetaWorldEstimatedKNext+= kalmanGain3X2[2][0]*innovationFront + kalmanGain3X2[2][1]*innovationLeft;
Ludwigfr 5:19f24c363418 1174
Ludwigfr 5:19f24c363418 1175 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 1176 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 1177 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 1178 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 1179 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 1180 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 1181 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 1182 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 1183 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 1184
Ludwigfr 5:19f24c363418 1185 break;
Ludwigfr 5:19f24c363418 1186 case 100://right
Ludwigfr 5:19f24c363418 1187
Ludwigfr 5:19f24c363418 1188 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 1189
Ludwigfr 5:19f24c363418 1190 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 1191 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 1192 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 1193
Ludwigfr 5:19f24c363418 1194 xEstimatedKNext+= kalmanGain3X1[0][0]*innovationRight;
Ludwigfr 5:19f24c363418 1195 yEstimatedKNext+= kalmanGain3X1[1][0]*innovationRight;
Ludwigfr 5:19f24c363418 1196 thetaWorldEstimatedKNext+= kalmanGain3X1[2][0]*innovationRight;
Ludwigfr 5:19f24c363418 1197
Ludwigfr 5:19f24c363418 1198 covariancePositionEstimationK[0][0]=- compositeInnovationCovariance1x1*pow(kalmanGain3X1[0][0],2) + covariancePositionEstimationK[0][0];
Ludwigfr 5:19f24c363418 1199 covariancePositionEstimationK[0][1]=covariancePositionEstimationK[0][1] - kalmanGain3X1[0][0]*kalmanGain3X1[1][0]*compositeInnovationCovariance1x1;
Ludwigfr 5:19f24c363418 1200 covariancePositionEstimationK[0][2]=covariancePositionEstimationK[0][2] - kalmanGain3X1[0][0]*kalmanGain3X1[2][0]*compositeInnovationCovariance1x1;
Ludwigfr 5:19f24c363418 1201 covariancePositionEstimationK[1][0]=covariancePositionEstimationK[1][0] - kalmanGain3X1[0][0]*kalmanGain3X1[1][0]*compositeInnovationCovariance1x1;
Ludwigfr 5:19f24c363418 1202 covariancePositionEstimationK[1][1]=- compositeInnovationCovariance1x1*pow(kalmanGain3X1[1][0],2) + covariancePositionEstimationK[1][1];
Ludwigfr 5:19f24c363418 1203 covariancePositionEstimationK[1][2]=covariancePositionEstimationK[1][2] - kalmanGain3X1[1][0]*kalmanGain3X1[2][0]*compositeInnovationCovariance1x1;
Ludwigfr 5:19f24c363418 1204 covariancePositionEstimationK[2][0]=covariancePositionEstimationK[2][0] - kalmanGain3X1[0][0]*kalmanGain3X1[2][0]*compositeInnovationCovariance1x1;
Ludwigfr 5:19f24c363418 1205 covariancePositionEstimationK[2][1]=covariancePositionEstimationK[2][1] - kalmanGain3X1[1][0]*kalmanGain3X1[2][0]*compositeInnovationCovariance1x1;
Ludwigfr 5:19f24c363418 1206 covariancePositionEstimationK[2][2]=- compositeInnovationCovariance1x1*pow(kalmanGain3X1[2][0],2) + covariancePositionEstimationK[2][2];
Ludwigfr 5:19f24c363418 1207
Ludwigfr 5:19f24c363418 1208 break;
Ludwigfr 5:19f24c363418 1209 case 101://right and left
Ludwigfr 5:19f24c363418 1210 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 1211 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 1212 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 1213 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 1214
Ludwigfr 5:19f24c363418 1215 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 1216 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 1217 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 1218 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 1219 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 1220 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 1221
Ludwigfr 5:19f24c363418 1222 xEstimatedKNext+= kalmanGain3X2[0][0]*innovationLeft + kalmanGain3X2[0][1]*innovationRight;
Ludwigfr 5:19f24c363418 1223 yEstimatedKNext+= kalmanGain3X2[1][0]*innovationLeft + kalmanGain3X2[1][1]*innovationRight;
Ludwigfr 5:19f24c363418 1224 thetaWorldEstimatedKNext+= kalmanGain3X2[2][0]*innovationLeft + kalmanGain3X2[2][1]*innovationRight;
Ludwigfr 5:19f24c363418 1225
Ludwigfr 5:19f24c363418 1226 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 1227 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 1228 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 1229 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 1230 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 1231 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 1232 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 1233 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 1234 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 1235
Ludwigfr 5:19f24c363418 1236 break;
Ludwigfr 5:19f24c363418 1237 case 110://right and front
Ludwigfr 5:19f24c363418 1238
Ludwigfr 5:19f24c363418 1239 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 1240 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 1241 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 1242 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 1243
Ludwigfr 5:19f24c363418 1244 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 1245 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 1246 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 1247 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 1248 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 1249 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 1250
Ludwigfr 5:19f24c363418 1251 xEstimatedKNext+= kalmanGain3X2[0][0]*innovationFront + kalmanGain3X2[0][1]*innovationRight;
Ludwigfr 5:19f24c363418 1252 yEstimatedKNext+= kalmanGain3X2[1][0]*innovationFront + kalmanGain3X2[1][1]*innovationRight;
Ludwigfr 5:19f24c363418 1253 thetaWorldEstimatedKNext+= kalmanGain3X2[2][0]*innovationFront + kalmanGain3X2[2][1]*innovationRight;
Ludwigfr 5:19f24c363418 1254
Ludwigfr 5:19f24c363418 1255 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 1256 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 1257 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 1258 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 1259 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 1260 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 1261 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 1262 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 1263 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 1264
Ludwigfr 5:19f24c363418 1265 break;
Ludwigfr 5:19f24c363418 1266 case 111://right front and left
Ludwigfr 5:19f24c363418 1267 //get the compositeInnovationCovariance3x3
Ludwigfr 5:19f24c363418 1268 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 1269 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 1270 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 1271 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 1272 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 1273 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 1274 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 1275 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 1276 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 1277
Ludwigfr 5:19f24c363418 1278 //compute the kalman gain
Ludwigfr 5:19f24c363418 1279 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 1280 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 1281 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 1282 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 1283 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 1284 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 1285 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 1286 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 1287 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 1288
Ludwigfr 5:19f24c363418 1289 //update the prediction
Ludwigfr 5:19f24c363418 1290 xEstimatedKNext+= kalmanGain3X3[0][0]*innovationFront + kalmanGain3X3[0][1]*innovationLeft + kalmanGain3X3[0][2]*innovationRight;
Ludwigfr 5:19f24c363418 1291 yEstimatedKNext+= kalmanGain3X3[1][0]*innovationFront + kalmanGain3X3[1][1]*innovationLeft + kalmanGain3X3[1][2]*innovationRight;
Ludwigfr 5:19f24c363418 1292 thetaWorldEstimatedKNext+= kalmanGain3X3[2][0]*innovationFront + kalmanGain3X3[2][1]*innovationLeft + kalmanGain3X3[2][2]*innovationRight;
Ludwigfr 5:19f24c363418 1293
Ludwigfr 5:19f24c363418 1294 //update covariancePositionEstimationK
Ludwigfr 5:19f24c363418 1295 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 1296 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 1297 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 1298 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 1299 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 1300 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 1301 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 1302 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 1303 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 1304
Ludwigfr 5:19f24c363418 1305 break;
Ludwigfr 5:19f24c363418 1306 }
Ludwigfr 5:19f24c363418 1307 //big question, in wich coordinate space are those measurements...
Ludwigfr 5:19f24c363418 1308 //try with world coordinate system
Ludwigfr 5:19f24c363418 1309
Ludwigfr 5:19f24c363418 1310 this->xWorld=xEstimatedKNext;
Ludwigfr 5:19f24c363418 1311 this->yWorld=yEstimatedKNext;
Ludwigfr 5:19f24c363418 1312 this->thetaWorld=thetaWorldEstimatedKNext;
Ludwigfr 5:19f24c363418 1313
Ludwigfr 5:19f24c363418 1314
Ludwigfr 5:19f24c363418 1315 //try with robot one
Ludwigfr 5:19f24c363418 1316 /*
Ludwigfr 5:19f24c363418 1317 X=xEstimatedKNext;
Ludwigfr 5:19f24c363418 1318 Y=yEstimatedKNext;
Ludwigfr 5:19f24c363418 1319 theta=thetaWorldEstimatedKNext;
Ludwigfr 5:19f24c363418 1320 this->xWorld=-Y;
Ludwigfr 5:19f24c363418 1321 this->yWorld=X;
Ludwigfr 5:19f24c363418 1322 if(theta >PI/2)
Ludwigfr 5:19f24c363418 1323 this->thetaWorld=-PI+(theta-PI/2);
Ludwigfr 5:19f24c363418 1324 else
Ludwigfr 5:19f24c363418 1325 this->thetaWorld=theta+PI/2;
Ludwigfr 5:19f24c363418 1326
Ludwigfr 5:19f24c363418 1327
Ludwigfr 5:19f24c363418 1328 this->print_map_with_robot_position();
Ludwigfr 5:19f24c363418 1329 pc.printf("\n\rX= %f",this->xWorld);
Ludwigfr 5:19f24c363418 1330 pc.printf("\n\rY= %f",this->yWorld);
Ludwigfr 5:19f24c363418 1331 pc.printf("\n\rtheta= %f",this->thetaWorld);
Ludwigfr 5:19f24c363418 1332 */
Ludwigfr 5:19f24c363418 1333 //update odometrie X Y theta...
Ludwigfr 5:19f24c363418 1334 }
Ludwigfr 5:19f24c363418 1335
Ludwigfr 5:19f24c363418 1336
Ludwigfr 5:19f24c363418 1337