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