Shuto Naruse / Mbed 2 deprecated Eurobot2012_Primary

Eurobot2012_Primary

Dependencies:   mbed Eurobot_2012_Primary

Eurobot_shared/Kalman/Kalman.cpp@26:0995f61cb7b8, 2012-10-17 (annotated)

Committer:
narshu
Date:
Wed Oct 17 22:22:47 2012 +0000
Revision:
26:0995f61cb7b8
Parent:
24:7a3906c2f5d5 
Eurobot 2012 Primary;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
narshu 24:7a3906c2f5d5 1 //***************************************************************************************
narshu 24:7a3906c2f5d5 2 //Kalman Filter implementation
narshu 24:7a3906c2f5d5 3 //***************************************************************************************
narshu 24:7a3906c2f5d5 4 #include "Kalman.h"
narshu 24:7a3906c2f5d5 5 #include "rtos.h"
narshu 24:7a3906c2f5d5 6 #include "RFSRF05.h"
narshu 24:7a3906c2f5d5 7 #include "math.h"
narshu 24:7a3906c2f5d5 8 #include "globals.h"
narshu 24:7a3906c2f5d5 9 #include "motors.h"
narshu 24:7a3906c2f5d5 10 #include "system.h"
narshu 24:7a3906c2f5d5 11 #include "geometryfuncs.h"
narshu 24:7a3906c2f5d5 12
narshu 24:7a3906c2f5d5 13 #include <tvmet/Matrix.h>
narshu 24:7a3906c2f5d5 14 #include <tvmet/Vector.h>
narshu 24:7a3906c2f5d5 15 using namespace tvmet;
narshu 24:7a3906c2f5d5 16
narshu 24:7a3906c2f5d5 17 Kalman::Kalman(Motors &motorsin,
narshu 24:7a3906c2f5d5 18 UI &uiin,
narshu 24:7a3906c2f5d5 19 PinName Sonar_Trig,
narshu 24:7a3906c2f5d5 20 PinName Sonar_Echo0,
narshu 24:7a3906c2f5d5 21 PinName Sonar_Echo1,
narshu 24:7a3906c2f5d5 22 PinName Sonar_Echo2,
narshu 24:7a3906c2f5d5 23 PinName Sonar_Echo3,
narshu 24:7a3906c2f5d5 24 PinName Sonar_Echo4,
narshu 24:7a3906c2f5d5 25 PinName Sonar_Echo5,
narshu 24:7a3906c2f5d5 26 PinName Sonar_SDI,
narshu 24:7a3906c2f5d5 27 PinName Sonar_SDO,
narshu 24:7a3906c2f5d5 28 PinName Sonar_SCK,
narshu 24:7a3906c2f5d5 29 PinName Sonar_NCS,
narshu 24:7a3906c2f5d5 30 PinName Sonar_NIRQ) :
narshu 24:7a3906c2f5d5 31 ir(*this),
narshu 24:7a3906c2f5d5 32 sonararray(Sonar_Trig,
narshu 24:7a3906c2f5d5 33 Sonar_Echo0,
narshu 24:7a3906c2f5d5 34 Sonar_Echo1,
narshu 24:7a3906c2f5d5 35 Sonar_Echo2,
narshu 24:7a3906c2f5d5 36 Sonar_Echo3,
narshu 24:7a3906c2f5d5 37 Sonar_Echo4,
narshu 24:7a3906c2f5d5 38 Sonar_Echo5,
narshu 24:7a3906c2f5d5 39 Sonar_SDI,
narshu 24:7a3906c2f5d5 40 Sonar_SDO,
narshu 24:7a3906c2f5d5 41 Sonar_SCK,
narshu 24:7a3906c2f5d5 42 Sonar_NCS,
narshu 24:7a3906c2f5d5 43 Sonar_NIRQ),
narshu 24:7a3906c2f5d5 44 motors(motorsin),
narshu 24:7a3906c2f5d5 45 ui(uiin),
narshu 24:7a3906c2f5d5 46 predictthread(predictloopwrapper, this, osPriorityNormal, 512),
narshu 24:7a3906c2f5d5 47 predictticker( SIGTICKARGS(predictthread, 0x1) ),
narshu 24:7a3906c2f5d5 48 // sonarthread(sonarloopwrapper, this, osPriorityNormal, 256),
narshu 24:7a3906c2f5d5 49 // sonarticker( SIGTICKARGS(sonarthread, 0x1) ),
narshu 24:7a3906c2f5d5 50 updatethread(updateloopwrapper, this, osPriorityNormal, 512) {
narshu 24:7a3906c2f5d5 51
narshu 24:7a3906c2f5d5 52 //Initilising offsets
narshu 24:7a3906c2f5d5 53 InitLock.lock();
narshu 24:7a3906c2f5d5 54 IR_Offset = 0;
narshu 24:7a3906c2f5d5 55 Sonar_Offset = 0;
narshu 24:7a3906c2f5d5 56 InitLock.unlock();
narshu 24:7a3906c2f5d5 57
narshu 24:7a3906c2f5d5 58
narshu 24:7a3906c2f5d5 59 //Initilising matrices
narshu 24:7a3906c2f5d5 60
narshu 24:7a3906c2f5d5 61 // X = x, y, theta;
narshu 24:7a3906c2f5d5 62 if (Colour)
narshu 24:7a3906c2f5d5 63 X = 0.5, 0, 0;
narshu 24:7a3906c2f5d5 64 else
narshu 24:7a3906c2f5d5 65 X = 2.5, 0, PI;
narshu 24:7a3906c2f5d5 66
narshu 24:7a3906c2f5d5 67 P = 1, 0, 0,
narshu 24:7a3906c2f5d5 68 0, 1, 0,
narshu 24:7a3906c2f5d5 69 0, 0, 0.04;
narshu 24:7a3906c2f5d5 70
narshu 24:7a3906c2f5d5 71 //measurment variance R is provided by each sensor when calling runupdate
narshu 24:7a3906c2f5d5 72
narshu 24:7a3906c2f5d5 73 //attach callback
narshu 24:7a3906c2f5d5 74 sonararray.callbackobj = (DummyCT*)this;
narshu 24:7a3906c2f5d5 75 sonararray.mcallbackfunc = (void (DummyCT::*)(int beaconnum, float distance, float variance)) &Kalman::runupdate;
narshu 24:7a3906c2f5d5 76
narshu 24:7a3906c2f5d5 77
narshu 24:7a3906c2f5d5 78 predictticker.start(20);
narshu 24:7a3906c2f5d5 79 // sonarticker.start(50);
narshu 24:7a3906c2f5d5 80
narshu 24:7a3906c2f5d5 81 }
narshu 24:7a3906c2f5d5 82
narshu 24:7a3906c2f5d5 83
narshu 24:7a3906c2f5d5 84 //Note: this init function assumes that the robot faces east, theta=0, in the +x direction
narshu 24:7a3906c2f5d5 85 void Kalman::KalmanInit() {
narshu 24:7a3906c2f5d5 86 motors.stop();
narshu 24:7a3906c2f5d5 87 float SonarMeasuresx1000[3];
narshu 24:7a3906c2f5d5 88 float IRMeasuresloc[3];
narshu 24:7a3906c2f5d5 89 int beacon_cnt = 0;
narshu 24:7a3906c2f5d5 90
narshu 24:7a3906c2f5d5 91
narshu 24:7a3906c2f5d5 92 // doesn't work since they break the ISR
narshu 24:7a3906c2f5d5 93 /*
narshu 24:7a3906c2f5d5 94 #ifdef ROBOT_PRIMARY
narshu 24:7a3906c2f5d5 95 LPC_UART3->FCR = LPC_UART3->FCR | 0x06; // Flush the serial FIFO buffer / OR with FCR
narshu 24:7a3906c2f5d5 96 #else
narshu 24:7a3906c2f5d5 97 LPC_UART1->FCR = LPC_UART1->FCR | 0x06; // Flush the serial FIFO buffer / OR with FCR
narshu 24:7a3906c2f5d5 98 #endif
narshu 24:7a3906c2f5d5 99 */
narshu 24:7a3906c2f5d5 100 // zeros the measurements
narshu 24:7a3906c2f5d5 101 for (int i = 0; i < 3; i++) {
narshu 24:7a3906c2f5d5 102 SonarMeasures[i] = 0;
narshu 24:7a3906c2f5d5 103 IRMeasures[i] = 0;
narshu 24:7a3906c2f5d5 104 }
narshu 24:7a3906c2f5d5 105
narshu 24:7a3906c2f5d5 106 InitLock.lock();
narshu 24:7a3906c2f5d5 107 //zeros offsets
narshu 24:7a3906c2f5d5 108 IR_Offset = 0;
narshu 24:7a3906c2f5d5 109 Sonar_Offset = 0;
narshu 24:7a3906c2f5d5 110 InitLock.unlock();
narshu 24:7a3906c2f5d5 111
narshu 24:7a3906c2f5d5 112 // attaches ir interrup
narshu 24:7a3906c2f5d5 113 ir.attachisr();
narshu 24:7a3906c2f5d5 114
narshu 24:7a3906c2f5d5 115 //wating untill the IR has reved up and picked up some valid data
narshu 24:7a3906c2f5d5 116 //Thread::wait(1000);
narshu 24:7a3906c2f5d5 117 wait(2);
narshu 24:7a3906c2f5d5 118
narshu 24:7a3906c2f5d5 119 //temporaraly disable IR updates
narshu 24:7a3906c2f5d5 120 ir.detachisr();
narshu 24:7a3906c2f5d5 121
narshu 24:7a3906c2f5d5 122 //lock the state throughout the computation, as we will override the state at the end
narshu 24:7a3906c2f5d5 123 InitLock.lock();
narshu 24:7a3906c2f5d5 124 statelock.lock();
narshu 24:7a3906c2f5d5 125
narshu 24:7a3906c2f5d5 126
narshu 24:7a3906c2f5d5 127
narshu 24:7a3906c2f5d5 128 SonarMeasuresx1000[0] = SonarMeasures[0]*1000.0f;
narshu 24:7a3906c2f5d5 129 SonarMeasuresx1000[1] = SonarMeasures[1]*1000.0f;
narshu 24:7a3906c2f5d5 130 SonarMeasuresx1000[2] = SonarMeasures[2]*1000.0f;
narshu 24:7a3906c2f5d5 131 IRMeasuresloc[0] = IRMeasures[0];
narshu 24:7a3906c2f5d5 132 IRMeasuresloc[1] = IRMeasures[1];
narshu 24:7a3906c2f5d5 133 IRMeasuresloc[2] = IRMeasures[2];
narshu 24:7a3906c2f5d5 134 //printf("0: %0.4f, 1: %0.4f, 2: %0.4f \n\r", IRMeasuresloc[0]*180/PI, IRMeasuresloc[1]*180/PI, IRMeasuresloc[2]*180/PI);
narshu 24:7a3906c2f5d5 135
narshu 24:7a3906c2f5d5 136 float d = beaconpos[2].y - beaconpos[1].y;
narshu 24:7a3906c2f5d5 137 float i = beaconpos[0].y - beaconpos[1].y;
narshu 24:7a3906c2f5d5 138 float j = beaconpos[0].x - beaconpos[1].x;
narshu 24:7a3906c2f5d5 139 float origin_x = beaconpos[1].x;
narshu 24:7a3906c2f5d5 140 float y_coor = (SonarMeasuresx1000[1]*SonarMeasuresx1000[1]- SonarMeasuresx1000[2]*SonarMeasuresx1000[2] + d*d) / (2*d);
narshu 24:7a3906c2f5d5 141 float x_coor = origin_x + (SonarMeasuresx1000[1]*SonarMeasuresx1000[1] - SonarMeasuresx1000[0]*SonarMeasuresx1000[0] + i*i + j*j)/(2*j) - i*y_coor/j;
narshu 24:7a3906c2f5d5 142
narshu 24:7a3906c2f5d5 143 //debug for trilateration
narshu 24:7a3906c2f5d5 144 printf("Cal at x: %0.4f, y: %0.4f \r\n",x_coor,y_coor );
narshu 24:7a3906c2f5d5 145
narshu 24:7a3906c2f5d5 146 float Dist_Exp[3];
narshu 24:7a3906c2f5d5 147 for (int i = 0; i < 3; i++) {
narshu 24:7a3906c2f5d5 148 //Compute sonar offset
narshu 24:7a3906c2f5d5 149 Dist_Exp[i] = hypot(beaconpos[i].y - y_coor,beaconpos[i].x - x_coor);
narshu 24:7a3906c2f5d5 150 Sonar_Offset += (SonarMeasuresx1000[i]-Dist_Exp[i])/3000.0f;
narshu 24:7a3906c2f5d5 151
narshu 24:7a3906c2f5d5 152 //Compute IR offset
narshu 24:7a3906c2f5d5 153 float angle_est = atan2(beaconpos[i].y - y_coor,beaconpos[i].x - x_coor);
narshu 24:7a3906c2f5d5 154 if (!Colour)
narshu 24:7a3906c2f5d5 155 angle_est -= PI;
narshu 24:7a3906c2f5d5 156 //printf("Angle %d : %f \n\r",i,angle_est*180/PI );
narshu 24:7a3906c2f5d5 157 // take average offset angle from valid readings
narshu 24:7a3906c2f5d5 158 if (IRMeasuresloc[i] != 0) {
narshu 24:7a3906c2f5d5 159 beacon_cnt ++;
narshu 24:7a3906c2f5d5 160 // changed to current angle - estimated angle
narshu 24:7a3906c2f5d5 161 float angle_temp = IRMeasuresloc[i] - angle_est;
narshu 24:7a3906c2f5d5 162 angle_temp -= (floor(angle_temp/(2*PI)))*2*PI;
narshu 24:7a3906c2f5d5 163 IR_Offset += angle_temp;
narshu 24:7a3906c2f5d5 164 }
narshu 24:7a3906c2f5d5 165 }
narshu 24:7a3906c2f5d5 166 IR_Offset /= float(beacon_cnt);
narshu 24:7a3906c2f5d5 167
narshu 24:7a3906c2f5d5 168 //debug
narshu 24:7a3906c2f5d5 169 printf("Offsets IR: %0.4f, Sonar: %0.4f \r\n",IR_Offset*180/PI,Sonar_Offset*1000 );
narshu 24:7a3906c2f5d5 170
narshu 24:7a3906c2f5d5 171 //statelock already locked
narshu 24:7a3906c2f5d5 172 X(0) = x_coor/1000.0f;
narshu 24:7a3906c2f5d5 173 X(1) = y_coor/1000.0f;
narshu 24:7a3906c2f5d5 174
narshu 24:7a3906c2f5d5 175 if (Colour)
narshu 24:7a3906c2f5d5 176 X(2) = 0;
narshu 24:7a3906c2f5d5 177 else
narshu 24:7a3906c2f5d5 178 X(2) = PI;
narshu 24:7a3906c2f5d5 179
narshu 24:7a3906c2f5d5 180 // unlocks mutexes
narshu 24:7a3906c2f5d5 181 InitLock.unlock();
narshu 24:7a3906c2f5d5 182 statelock.unlock();
narshu 24:7a3906c2f5d5 183
narshu 24:7a3906c2f5d5 184
narshu 24:7a3906c2f5d5 185 //reattach the IR processing
narshu 24:7a3906c2f5d5 186 ir.attachisr();
narshu 24:7a3906c2f5d5 187 }
narshu 24:7a3906c2f5d5 188
narshu 24:7a3906c2f5d5 189
narshu 24:7a3906c2f5d5 190 void Kalman::predictloop() {
narshu 24:7a3906c2f5d5 191
narshu 24:7a3906c2f5d5 192 OLED4 = !ui.regid(0, 3);
narshu 24:7a3906c2f5d5 193 OLED4 = !ui.regid(1, 4);
narshu 24:7a3906c2f5d5 194
narshu 24:7a3906c2f5d5 195 float lastleft = 0;
narshu 24:7a3906c2f5d5 196 float lastright = 0;
narshu 24:7a3906c2f5d5 197
narshu 24:7a3906c2f5d5 198 while (1) {
narshu 24:7a3906c2f5d5 199 Thread::signal_wait(0x1);
narshu 24:7a3906c2f5d5 200 OLED1 = !OLED1;
narshu 24:7a3906c2f5d5 201
narshu 24:7a3906c2f5d5 202 int leftenc = motors.getEncoder1();
narshu 24:7a3906c2f5d5 203 int rightenc = motors.getEncoder2();
narshu 24:7a3906c2f5d5 204
narshu 24:7a3906c2f5d5 205 float dleft = motors.encoderToDistance(leftenc-lastleft)/1000.0f;
narshu 24:7a3906c2f5d5 206 float dright = motors.encoderToDistance(rightenc-lastright)/1000.0f;
narshu 24:7a3906c2f5d5 207
narshu 24:7a3906c2f5d5 208 lastleft = leftenc;
narshu 24:7a3906c2f5d5 209 lastright = rightenc;
narshu 24:7a3906c2f5d5 210
narshu 24:7a3906c2f5d5 211
narshu 24:7a3906c2f5d5 212 //The below calculation are in body frame (where +x is forward)
narshu 24:7a3906c2f5d5 213 float dxp, dyp,d,r;
narshu 24:7a3906c2f5d5 214 float thetap = (dright - dleft)*PI / (float(robotCircumference)/1000.0f);
narshu 24:7a3906c2f5d5 215 if (abs(thetap) < 0.02) { //if the rotation through the integration step is small, approximate with a straight line to avoid numerical error
narshu 24:7a3906c2f5d5 216 d = (dright + dleft)/2.0f;
narshu 24:7a3906c2f5d5 217 dxp = d*cos(thetap/2.0f);
narshu 24:7a3906c2f5d5 218 dyp = d*sin(thetap/2.0f);
narshu 24:7a3906c2f5d5 219
narshu 24:7a3906c2f5d5 220 } else { //calculate circle arc
narshu 24:7a3906c2f5d5 221 //float r = (right + left) / (4.0f * PI * thetap);
narshu 24:7a3906c2f5d5 222 r = (dright + dleft) / (2.0f*thetap);
narshu 24:7a3906c2f5d5 223 dxp = abs(r)*sin(thetap);
narshu 24:7a3906c2f5d5 224 dyp = r - r*cos(thetap);
narshu 24:7a3906c2f5d5 225 }
narshu 24:7a3906c2f5d5 226
narshu 24:7a3906c2f5d5 227 statelock.lock();
narshu 24:7a3906c2f5d5 228
narshu 24:7a3906c2f5d5 229 float tempX2 = X(2);
narshu 24:7a3906c2f5d5 230 //rotating to cartesian frame and updating state
narshu 24:7a3906c2f5d5 231 X(0) += dxp * cos(X(2)) - dyp * sin(X(2));
narshu 24:7a3906c2f5d5 232 X(1) += dxp * sin(X(2)) + dyp * cos(X(2));
narshu 24:7a3906c2f5d5 233 X(2) = rectifyAng(X(2) + thetap);
narshu 24:7a3906c2f5d5 234
narshu 24:7a3906c2f5d5 235 //Linearising F around X
narshu 24:7a3906c2f5d5 236 float avgX2 = (X(2) + tempX2)/2.0f;
narshu 24:7a3906c2f5d5 237 Matrix<float, 3, 3> F;
narshu 24:7a3906c2f5d5 238 F = 1, 0, (dxp * -sin(avgX2) - dyp * cos(avgX2)),
narshu 24:7a3906c2f5d5 239 0, 1, (dxp * cos(avgX2) - dyp * sin(avgX2)),
narshu 24:7a3906c2f5d5 240 0, 0, 1;
narshu 24:7a3906c2f5d5 241
narshu 24:7a3906c2f5d5 242 //Generating forward and rotational variance
narshu 24:7a3906c2f5d5 243 float varfwd = fwdvarperunit * abs(dright + dleft) / 2.0f;
narshu 24:7a3906c2f5d5 244 float varang = varperang * abs(thetap);
narshu 24:7a3906c2f5d5 245 float varxydt = xyvarpertime * PREDICTPERIOD/1000.0f;
narshu 24:7a3906c2f5d5 246 float varangdt = angvarpertime * PREDICTPERIOD/1000.0f;
narshu 24:7a3906c2f5d5 247
narshu 24:7a3906c2f5d5 248 //Rotating into cartesian frame
narshu 24:7a3906c2f5d5 249 Matrix<float, 2, 2> Qsub,Qsubrot,Qrot;
narshu 24:7a3906c2f5d5 250 Qsub = varfwd + varxydt, 0,
narshu 24:7a3906c2f5d5 251 0, varxydt;
narshu 24:7a3906c2f5d5 252
narshu 24:7a3906c2f5d5 253 Qrot = Rotmatrix(X(2));
narshu 24:7a3906c2f5d5 254
narshu 24:7a3906c2f5d5 255 Qsubrot = Qrot * Qsub * trans(Qrot);
narshu 24:7a3906c2f5d5 256
narshu 24:7a3906c2f5d5 257 //Generate Q
narshu 24:7a3906c2f5d5 258 Matrix<float, 3, 3> Q;//(Qsubrot);
narshu 24:7a3906c2f5d5 259 Q = Qsubrot(0,0), Qsubrot(0,1), 0,
narshu 24:7a3906c2f5d5 260 Qsubrot(1,0), Qsubrot(1,1), 0,
narshu 24:7a3906c2f5d5 261 0, 0, varang + varangdt;
narshu 24:7a3906c2f5d5 262
narshu 24:7a3906c2f5d5 263 P = F * P * trans(F) + Q;
narshu 24:7a3906c2f5d5 264
narshu 24:7a3906c2f5d5 265 //Update UI
narshu 24:7a3906c2f5d5 266 float statecpy[] = {X(0), X(1), X(2)};
narshu 24:7a3906c2f5d5 267 ui.updateval(0, statecpy, 3);
narshu 24:7a3906c2f5d5 268
narshu 24:7a3906c2f5d5 269 float Pcpy[] = {P(0,0), P(0,1), P(1,0), P(1,1)};
narshu 24:7a3906c2f5d5 270 ui.updateval(1, Pcpy, 4);
narshu 24:7a3906c2f5d5 271
narshu 24:7a3906c2f5d5 272 statelock.unlock();
narshu 24:7a3906c2f5d5 273 }
narshu 24:7a3906c2f5d5 274 }
narshu 24:7a3906c2f5d5 275
narshu 24:7a3906c2f5d5 276 //void Kalman::sonarloop() {
narshu 24:7a3906c2f5d5 277 // while (1) {
narshu 24:7a3906c2f5d5 278 // Thread::signal_wait(0x1);
narshu 24:7a3906c2f5d5 279 // sonararray.startRange();
narshu 24:7a3906c2f5d5 280 // }
narshu 24:7a3906c2f5d5 281 //}
narshu 24:7a3906c2f5d5 282
narshu 24:7a3906c2f5d5 283
narshu 24:7a3906c2f5d5 284 void Kalman::runupdate(measurement_t type, float value, float variance) {
narshu 24:7a3906c2f5d5 285 //printf("beacon %d dist %f\r\n", sonarid, dist);
narshu 24:7a3906c2f5d5 286 //led2 = !led2;
narshu 24:7a3906c2f5d5 287
narshu 24:7a3906c2f5d5 288 measurmentdata* measured = (measurmentdata*)measureMQ.alloc();
narshu 24:7a3906c2f5d5 289 if (measured) {
narshu 24:7a3906c2f5d5 290 measured->mtype = type;
narshu 24:7a3906c2f5d5 291 measured->value = value;
narshu 24:7a3906c2f5d5 292 measured->variance = variance;
narshu 24:7a3906c2f5d5 293
narshu 24:7a3906c2f5d5 294 osStatus putret = measureMQ.put(measured);
narshu 24:7a3906c2f5d5 295 if (putret)
narshu 24:7a3906c2f5d5 296 OLED4 = 1;
narshu 24:7a3906c2f5d5 297 // printf("putting in MQ error code %#x\r\n", putret);
narshu 24:7a3906c2f5d5 298 } else {
narshu 24:7a3906c2f5d5 299 OLED4 = 1;
narshu 24:7a3906c2f5d5 300 //printf("MQalloc returned NULL ptr\r\n");
narshu 24:7a3906c2f5d5 301 }
narshu 24:7a3906c2f5d5 302
narshu 24:7a3906c2f5d5 303 }
narshu 24:7a3906c2f5d5 304
narshu 24:7a3906c2f5d5 305 void Kalman::updateloop() {
narshu 24:7a3906c2f5d5 306
narshu 24:7a3906c2f5d5 307 //sonar Y chanels
narshu 24:7a3906c2f5d5 308 ui.regid(2, 1);
narshu 24:7a3906c2f5d5 309 ui.regid(3, 1);
narshu 24:7a3906c2f5d5 310 ui.regid(4, 1);
narshu 24:7a3906c2f5d5 311
narshu 24:7a3906c2f5d5 312 //IR Y chanels
narshu 24:7a3906c2f5d5 313 ui.regid(5, 1);
narshu 24:7a3906c2f5d5 314 ui.regid(6, 1);
narshu 24:7a3906c2f5d5 315 ui.regid(7, 1);
narshu 24:7a3906c2f5d5 316
narshu 24:7a3906c2f5d5 317 measurement_t type;
narshu 24:7a3906c2f5d5 318 float value,variance,rbx,rby,expecdist,Y;
narshu 24:7a3906c2f5d5 319 float dhdx,dhdy;
narshu 24:7a3906c2f5d5 320 bool aborton2stddev = false;
narshu 24:7a3906c2f5d5 321
narshu 24:7a3906c2f5d5 322 Matrix<float, 1, 3> H;
narshu 24:7a3906c2f5d5 323
narshu 24:7a3906c2f5d5 324 float S;
narshu 24:7a3906c2f5d5 325 Matrix<float, 3, 3> I3( identity< Matrix<float, 3, 3> >() );
narshu 24:7a3906c2f5d5 326
narshu 24:7a3906c2f5d5 327
narshu 24:7a3906c2f5d5 328 while (1) {
narshu 24:7a3906c2f5d5 329 OLED2 = !OLED2;
narshu 24:7a3906c2f5d5 330
narshu 24:7a3906c2f5d5 331 osEvent evt = measureMQ.get();
narshu 24:7a3906c2f5d5 332
narshu 24:7a3906c2f5d5 333 if (evt.status == osEventMail) {
narshu 24:7a3906c2f5d5 334
narshu 24:7a3906c2f5d5 335 measurmentdata &measured = *(measurmentdata*)evt.value.p;
narshu 24:7a3906c2f5d5 336 type = measured.mtype; //Note, may support more measurment types than sonar in the future!
narshu 24:7a3906c2f5d5 337 value = measured.value;
narshu 24:7a3906c2f5d5 338 variance = measured.variance;
narshu 24:7a3906c2f5d5 339
narshu 24:7a3906c2f5d5 340 // don't forget to free the memory
narshu 24:7a3906c2f5d5 341 measureMQ.free(&measured);
narshu 24:7a3906c2f5d5 342
narshu 24:7a3906c2f5d5 343 if (type <= maxmeasure) {
narshu 24:7a3906c2f5d5 344
narshu 24:7a3906c2f5d5 345 if (type <= SONAR3) {
narshu 24:7a3906c2f5d5 346
narshu 24:7a3906c2f5d5 347 InitLock.lock();
narshu 24:7a3906c2f5d5 348 float dist = value / 1000.0f - Sonar_Offset; //converting to m from mm,subtract the offset
narshu 24:7a3906c2f5d5 349 InitLock.unlock();
narshu 24:7a3906c2f5d5 350
narshu 24:7a3906c2f5d5 351 int sonarid = type;
narshu 24:7a3906c2f5d5 352 aborton2stddev = true;
narshu 24:7a3906c2f5d5 353
narshu 24:7a3906c2f5d5 354 statelock.lock();
narshu 24:7a3906c2f5d5 355 //update the current sonar readings
narshu 24:7a3906c2f5d5 356 SonarMeasures[sonarid] = dist;
narshu 24:7a3906c2f5d5 357
narshu 24:7a3906c2f5d5 358 rbx = X(0) - beaconpos[sonarid].x/1000.0f;
narshu 24:7a3906c2f5d5 359 rby = X(1) - beaconpos[sonarid].y/1000.0f;
narshu 24:7a3906c2f5d5 360
narshu 24:7a3906c2f5d5 361 expecdist = hypot(rbx, rby);//sqrt(rbx*rbx + rby*rby);
narshu 24:7a3906c2f5d5 362 Y = dist - expecdist;
narshu 24:7a3906c2f5d5 363
narshu 24:7a3906c2f5d5 364 //send to ui
narshu 24:7a3906c2f5d5 365 ui.updateval(sonarid+2, Y);
narshu 24:7a3906c2f5d5 366
narshu 24:7a3906c2f5d5 367 dhdx = rbx / expecdist;
narshu 24:7a3906c2f5d5 368 dhdy = rby / expecdist;
narshu 24:7a3906c2f5d5 369
narshu 24:7a3906c2f5d5 370 H = dhdx, dhdy, 0;
narshu 24:7a3906c2f5d5 371
narshu 24:7a3906c2f5d5 372 } else if (type <= IR3) {
narshu 24:7a3906c2f5d5 373
narshu 24:7a3906c2f5d5 374 aborton2stddev = false;
narshu 24:7a3906c2f5d5 375 int IRidx = type-3;
narshu 24:7a3906c2f5d5 376
narshu 24:7a3906c2f5d5 377 // subtract the IR offset
narshu 24:7a3906c2f5d5 378 InitLock.lock();
narshu 24:7a3906c2f5d5 379 value -= IR_Offset;
narshu 24:7a3906c2f5d5 380 InitLock.unlock();
narshu 24:7a3906c2f5d5 381
narshu 24:7a3906c2f5d5 382 statelock.lock();
narshu 24:7a3906c2f5d5 383 IRMeasures[IRidx] = value;
narshu 24:7a3906c2f5d5 384
narshu 24:7a3906c2f5d5 385 rbx = X(0) - beaconpos[IRidx].x/1000.0f;
narshu 24:7a3906c2f5d5 386 rby = X(1) - beaconpos[IRidx].y/1000.0f;
narshu 24:7a3906c2f5d5 387
narshu 24:7a3906c2f5d5 388 float expecang = atan2(-rby, -rbx) - X(2);
narshu 24:7a3906c2f5d5 389 Y = rectifyAng(value - expecang);
narshu 24:7a3906c2f5d5 390
narshu 24:7a3906c2f5d5 391 //send to ui
narshu 24:7a3906c2f5d5 392 ui.updateval(IRidx + 5, Y);
narshu 24:7a3906c2f5d5 393
narshu 24:7a3906c2f5d5 394 float dstsq = rbx*rbx + rby*rby;
narshu 24:7a3906c2f5d5 395 H = -rby/dstsq, rbx/dstsq, -1;
narshu 24:7a3906c2f5d5 396 }
narshu 24:7a3906c2f5d5 397
narshu 24:7a3906c2f5d5 398 Matrix<float, 3, 1> PH (P * trans(H));
narshu 24:7a3906c2f5d5 399 S = (H * PH)(0,0) + variance;
narshu 24:7a3906c2f5d5 400
narshu 24:7a3906c2f5d5 401 if (aborton2stddev && Y*Y > 4 * S) {
narshu 24:7a3906c2f5d5 402 statelock.unlock();
narshu 24:7a3906c2f5d5 403 continue;
narshu 24:7a3906c2f5d5 404 }
narshu 24:7a3906c2f5d5 405
narshu 24:7a3906c2f5d5 406 Matrix<float, 3, 1> K (PH * (1/S));
narshu 24:7a3906c2f5d5 407
narshu 24:7a3906c2f5d5 408 //Updating state
narshu 24:7a3906c2f5d5 409 X += col(K, 0) * Y;
narshu 24:7a3906c2f5d5 410 X(2) = rectifyAng(X(2));
narshu 24:7a3906c2f5d5 411
narshu 24:7a3906c2f5d5 412 P = (I3 - K * H) * P;
narshu 24:7a3906c2f5d5 413
narshu 24:7a3906c2f5d5 414 statelock.unlock();
narshu 24:7a3906c2f5d5 415
narshu 24:7a3906c2f5d5 416 }
narshu 24:7a3906c2f5d5 417
narshu 24:7a3906c2f5d5 418 } else {
narshu 24:7a3906c2f5d5 419 OLED4 = 1;
narshu 24:7a3906c2f5d5 420 //printf("ERROR: in updateloop, code %#x", evt);
narshu 24:7a3906c2f5d5 421 }
narshu 24:7a3906c2f5d5 422
narshu 24:7a3906c2f5d5 423 }
narshu 24:7a3906c2f5d5 424
narshu 24:7a3906c2f5d5 425 }
narshu 24:7a3906c2f5d5 426
narshu 24:7a3906c2f5d5 427 // reset kalman states
narshu 24:7a3906c2f5d5 428 void Kalman::KalmanReset() {
narshu 24:7a3906c2f5d5 429 float SonarMeasuresx1000[3];
narshu 24:7a3906c2f5d5 430 statelock.lock();
narshu 24:7a3906c2f5d5 431 SonarMeasuresx1000[0] = SonarMeasures[0]*1000.0f;
narshu 24:7a3906c2f5d5 432 SonarMeasuresx1000[1] = SonarMeasures[1]*1000.0f;
narshu 24:7a3906c2f5d5 433 SonarMeasuresx1000[2] = SonarMeasures[2]*1000.0f;
narshu 24:7a3906c2f5d5 434 //printf("0: %0.4f, 1: %0.4f, 2: %0.4f \n\r", IRMeasuresloc[0]*180/PI, IRMeasuresloc[1]*180/PI, IRMeasuresloc[2]*180/PI);
narshu 24:7a3906c2f5d5 435
narshu 24:7a3906c2f5d5 436 float d = beaconpos[2].y - beaconpos[1].y;
narshu 24:7a3906c2f5d5 437 float i = beaconpos[0].y - beaconpos[1].y;
narshu 24:7a3906c2f5d5 438 float j = beaconpos[0].x - beaconpos[1].x;
narshu 24:7a3906c2f5d5 439 float origin_x = beaconpos[1].x;
narshu 24:7a3906c2f5d5 440 float y_coor = (SonarMeasuresx1000[1]*SonarMeasuresx1000[1]- SonarMeasuresx1000[2]*SonarMeasuresx1000[2] + d*d) / (2*d);
narshu 24:7a3906c2f5d5 441 float x_coor = origin_x +(SonarMeasuresx1000[1]*SonarMeasuresx1000[1] - SonarMeasuresx1000[0]*SonarMeasuresx1000[0] + i*i + j*j)/(2*j) - i*y_coor/j;
narshu 24:7a3906c2f5d5 442
narshu 24:7a3906c2f5d5 443 //statelock already locked
narshu 24:7a3906c2f5d5 444 X(0) = x_coor/1000.0f;
narshu 24:7a3906c2f5d5 445 X(1) = y_coor/1000.0f;
narshu 24:7a3906c2f5d5 446
narshu 24:7a3906c2f5d5 447
narshu 24:7a3906c2f5d5 448
narshu 24:7a3906c2f5d5 449 /* if (Colour){
narshu 24:7a3906c2f5d5 450 X(0) = 0.2;
narshu 24:7a3906c2f5d5 451 X(1) = 0.2;
narshu 24:7a3906c2f5d5 452 //X(2) = 0;
narshu 24:7a3906c2f5d5 453 }
narshu 24:7a3906c2f5d5 454 else {
narshu 24:7a3906c2f5d5 455 X(0) = 2.8;
narshu 24:7a3906c2f5d5 456 X(1) = 0.2;
narshu 24:7a3906c2f5d5 457 //X(2) = PI;
narshu 24:7a3906c2f5d5 458 }
narshu 24:7a3906c2f5d5 459 */
narshu 24:7a3906c2f5d5 460 P = 0.05, 0, 0,
narshu 24:7a3906c2f5d5 461 0, 0.05, 0,
narshu 24:7a3906c2f5d5 462 0, 0, 0.04;
narshu 24:7a3906c2f5d5 463
narshu 24:7a3906c2f5d5 464 // unlocks mutexes
narshu 24:7a3906c2f5d5 465 statelock.unlock();
narshu 24:7a3906c2f5d5 466
narshu 24:7a3906c2f5d5 467 }