Shuto Naruse
/
Kalman
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Kalman.cpp@0:e238496b8073, 2012-04-26 (annotated)
- Committer:
- narshu
- Date:
- Thu Apr 26 19:11:11 2012 +0000
- Revision:
- 0:e238496b8073
- Child:
- 1:4964fa534202
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| narshu | 0:e238496b8073 | 1 | //*************************************************************************************** |
| narshu | 0:e238496b8073 | 2 | //Kalman Filter implementation |
| narshu | 0:e238496b8073 | 3 | //*************************************************************************************** |
| narshu | 0:e238496b8073 | 4 | #include "Kalman.h" |
| narshu | 0:e238496b8073 | 5 | #include "rtos.h" |
| narshu | 0:e238496b8073 | 6 | #include "RFSRF05.h" |
| narshu | 0:e238496b8073 | 7 | #include "math.h" |
| narshu | 0:e238496b8073 | 8 | #include "globals.h" |
| narshu | 0:e238496b8073 | 9 | #include "motors.h" |
| narshu | 0:e238496b8073 | 10 | #include "system.h" |
| narshu | 0:e238496b8073 | 11 | #include "geometryfuncs.h" |
| narshu | 0:e238496b8073 | 12 | |
| narshu | 0:e238496b8073 | 13 | #include <tvmet/Matrix.h> |
| narshu | 0:e238496b8073 | 14 | #include <tvmet/Vector.h> |
| narshu | 0:e238496b8073 | 15 | using namespace tvmet; |
| narshu | 0:e238496b8073 | 16 | |
| narshu | 0:e238496b8073 | 17 | Kalman::Kalman(Motors &motorsin) : |
| narshu | 0:e238496b8073 | 18 | ir(*this), |
| narshu | 0:e238496b8073 | 19 | sonararray(p10,p21,p22,p23,p24,p25,p26,p5,p6,p7,p8,p9), |
| narshu | 0:e238496b8073 | 20 | motors(motorsin), |
| narshu | 0:e238496b8073 | 21 | predictthread(predictloopwrapper, this, osPriorityNormal, 512), |
| narshu | 0:e238496b8073 | 22 | predictticker( SIGTICKARGS(predictthread, 0x1) ), |
| narshu | 0:e238496b8073 | 23 | // sonarthread(sonarloopwrapper, this, osPriorityNormal, 256), |
| narshu | 0:e238496b8073 | 24 | // sonarticker( SIGTICKARGS(sonarthread, 0x1) ), |
| narshu | 0:e238496b8073 | 25 | updatethread(updateloopwrapper, this, osPriorityNormal, 512) { |
| narshu | 0:e238496b8073 | 26 | |
| narshu | 0:e238496b8073 | 27 | Kalman_init = false; |
| narshu | 0:e238496b8073 | 28 | //Intialising some arrays to zero |
| narshu | 0:e238496b8073 | 29 | for (int kk = 0; kk < 3; kk ++) { |
| narshu | 0:e238496b8073 | 30 | SonarMeasure_Offset[kk] = 0; |
| narshu | 0:e238496b8073 | 31 | } |
| narshu | 0:e238496b8073 | 32 | //Initialising other vars |
| narshu | 0:e238496b8073 | 33 | |
| narshu | 0:e238496b8073 | 34 | |
| narshu | 0:e238496b8073 | 35 | //Initilising matrices |
| narshu | 0:e238496b8073 | 36 | |
| narshu | 0:e238496b8073 | 37 | // X = x, y, theta; |
| narshu | 0:e238496b8073 | 38 | X = 0.5, 0, 0; |
| narshu | 0:e238496b8073 | 39 | |
| narshu | 0:e238496b8073 | 40 | P = 1, 0, 0, |
| narshu | 0:e238496b8073 | 41 | 0, 1, 0, |
| narshu | 0:e238496b8073 | 42 | 0, 0, 0.04; |
| narshu | 0:e238496b8073 | 43 | |
| narshu | 0:e238496b8073 | 44 | //measurment variance R is provided by each sensor when calling runupdate |
| narshu | 0:e238496b8073 | 45 | |
| narshu | 0:e238496b8073 | 46 | //attach callback |
| narshu | 0:e238496b8073 | 47 | sonararray.callbackobj = (DummyCT*)this; |
| narshu | 0:e238496b8073 | 48 | sonararray.mcallbackfunc = (void (DummyCT::*)(int beaconnum, float distance, float variance)) &Kalman::runupdate; |
| narshu | 0:e238496b8073 | 49 | |
| narshu | 0:e238496b8073 | 50 | |
| narshu | 0:e238496b8073 | 51 | predictticker.start(20); |
| narshu | 0:e238496b8073 | 52 | // sonarticker.start(50); |
| narshu | 0:e238496b8073 | 53 | |
| narshu | 0:e238496b8073 | 54 | } |
| narshu | 0:e238496b8073 | 55 | |
| narshu | 0:e238496b8073 | 56 | |
| narshu | 0:e238496b8073 | 57 | //Note: this init function assumes that the robot faces east, theta=0, in the +x direction |
| narshu | 0:e238496b8073 | 58 | void Kalman::KalmanInit() { |
| narshu | 0:e238496b8073 | 59 | float SonarMeasuresx1000[3]; |
| narshu | 0:e238496b8073 | 60 | float IRMeasuresloc[3]; |
| narshu | 0:e238496b8073 | 61 | int beacon_cnt = 0; |
| narshu | 0:e238496b8073 | 62 | // set initiating flag to false |
| narshu | 0:e238496b8073 | 63 | Kalman_init = false; |
| narshu | 0:e238496b8073 | 64 | |
| narshu | 0:e238496b8073 | 65 | // init the offset array |
| narshu | 0:e238496b8073 | 66 | for (int k = 0; k < 3; k ++) { |
| narshu | 0:e238496b8073 | 67 | SonarMeasure_Offset[k] = 0; |
| narshu | 0:e238496b8073 | 68 | IRMeasures[k] = 0; |
| narshu | 0:e238496b8073 | 69 | } |
| narshu | 0:e238496b8073 | 70 | |
| narshu | 0:e238496b8073 | 71 | LPC_UART0->FCR = LPC_UART0->FCR | 0x06; // Flush the serial FIFO buffer / OR with FCR |
| narshu | 0:e238496b8073 | 72 | //wating untill the IR has reved up and picked up some data |
| narshu | 0:e238496b8073 | 73 | wait(1); |
| narshu | 0:e238496b8073 | 74 | |
| narshu | 0:e238496b8073 | 75 | //temporaraly disable IR updates |
| narshu | 0:e238496b8073 | 76 | ir.detachisr(); |
| narshu | 0:e238496b8073 | 77 | //IRturret.attach(NULL,Serial::RxIrq); |
| narshu | 0:e238496b8073 | 78 | |
| narshu | 0:e238496b8073 | 79 | //lock the state throughout the computation, as we will override the state at the end |
| narshu | 0:e238496b8073 | 80 | statelock.lock(); |
| narshu | 0:e238496b8073 | 81 | |
| narshu | 0:e238496b8073 | 82 | SonarMeasuresx1000[0] = SonarMeasures[0]*1000.0f; |
| narshu | 0:e238496b8073 | 83 | SonarMeasuresx1000[1] = SonarMeasures[1]*1000.0f; |
| narshu | 0:e238496b8073 | 84 | SonarMeasuresx1000[2] = SonarMeasures[2]*1000.0f; |
| narshu | 0:e238496b8073 | 85 | IRMeasuresloc[0] = IRMeasures[0]; |
| narshu | 0:e238496b8073 | 86 | IRMeasuresloc[1] = IRMeasures[1]; |
| narshu | 0:e238496b8073 | 87 | IRMeasuresloc[2] = IRMeasures[2]; |
| narshu | 0:e238496b8073 | 88 | //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:e238496b8073 | 89 | |
| narshu | 0:e238496b8073 | 90 | float d = beaconpos[2].y - beaconpos[1].y; |
| narshu | 0:e238496b8073 | 91 | float i = beaconpos[0].y - beaconpos[1].y; |
| narshu | 0:e238496b8073 | 92 | float j = beaconpos[0].x - beaconpos[1].x; |
| narshu | 0:e238496b8073 | 93 | float y_coor = (SonarMeasuresx1000[1]*SonarMeasuresx1000[1]- SonarMeasuresx1000[2]*SonarMeasuresx1000[2] + d*d) / (2*d); |
| narshu | 0:e238496b8073 | 94 | float x_coor = (SonarMeasuresx1000[1]*SonarMeasuresx1000[1] - SonarMeasuresx1000[0]*SonarMeasuresx1000[0] + i*i + j*j)/(2*j) - i*y_coor/j; |
| narshu | 0:e238496b8073 | 95 | |
| narshu | 0:e238496b8073 | 96 | //Compute sonar offset |
| narshu | 0:e238496b8073 | 97 | float Dist_Exp[3]; |
| narshu | 0:e238496b8073 | 98 | for (int k = 0; k < 3; k++) { |
| narshu | 0:e238496b8073 | 99 | Dist_Exp[k] = sqrt((beaconpos[k].y - y_coor)*(beaconpos[k].y - y_coor)+(beaconpos[k].x - x_coor)*(beaconpos[k].x - x_coor)); |
| narshu | 0:e238496b8073 | 100 | SonarMeasure_Offset[k] = (SonarMeasuresx1000[k]-Dist_Exp[k])/1000.0f; |
| narshu | 0:e238496b8073 | 101 | } |
| narshu | 0:e238496b8073 | 102 | |
| narshu | 0:e238496b8073 | 103 | //Compute IR offset |
| narshu | 0:e238496b8073 | 104 | ir.angleOffset = 0; |
| narshu | 0:e238496b8073 | 105 | for (int i = 0; i < 3; i++) { |
| narshu | 0:e238496b8073 | 106 | float angle_est = atan2(beaconpos[i].y - y_coor,beaconpos[i].x - x_coor); |
| narshu | 0:e238496b8073 | 107 | // take average offset angle from valid readings |
| narshu | 0:e238496b8073 | 108 | if (IRMeasuresloc[i] != 0) { |
| narshu | 0:e238496b8073 | 109 | beacon_cnt ++; |
| narshu | 0:e238496b8073 | 110 | // changed to current angle - estimated angle |
| narshu | 0:e238496b8073 | 111 | float angle_temp = IRMeasuresloc[i] - angle_est; |
| narshu | 0:e238496b8073 | 112 | angle_temp -= (floor(angle_temp/(2*PI)))*2*PI; |
| narshu | 0:e238496b8073 | 113 | ir.angleOffset += angle_temp; |
| narshu | 0:e238496b8073 | 114 | } |
| narshu | 0:e238496b8073 | 115 | } |
| narshu | 0:e238496b8073 | 116 | ir.angleOffset = ir.angleOffset/float(beacon_cnt); |
| narshu | 0:e238496b8073 | 117 | //printf("\n\r"); |
| narshu | 0:e238496b8073 | 118 | |
| narshu | 0:e238496b8073 | 119 | //statelock already locked |
| narshu | 0:e238496b8073 | 120 | ir.angleInit = true; |
| narshu | 0:e238496b8073 | 121 | // set int flag to true |
| narshu | 0:e238496b8073 | 122 | Kalman_init = true; |
| narshu | 0:e238496b8073 | 123 | X(0) = x_coor/1000.0f; |
| narshu | 0:e238496b8073 | 124 | X(1) = y_coor/1000.0f; |
| narshu | 0:e238496b8073 | 125 | X(2) = 0; |
| narshu | 0:e238496b8073 | 126 | statelock.unlock(); |
| narshu | 0:e238496b8073 | 127 | |
| narshu | 0:e238496b8073 | 128 | //printf("x: %0.4f, y: %0.4f, offset: %0.4f \n\r", x_coor, y_coor, angleOffset*180/PI); |
| narshu | 0:e238496b8073 | 129 | |
| narshu | 0:e238496b8073 | 130 | //reattach the IR processing |
| narshu | 0:e238496b8073 | 131 | ir.attachisr(); |
| narshu | 0:e238496b8073 | 132 | //IRturret.attach(&IR::vIRValueISR,Serial::RxIrq); |
| narshu | 0:e238496b8073 | 133 | } |
| narshu | 0:e238496b8073 | 134 | |
| narshu | 0:e238496b8073 | 135 | |
| narshu | 0:e238496b8073 | 136 | void Kalman::predictloop() { |
| narshu | 0:e238496b8073 | 137 | |
| narshu | 0:e238496b8073 | 138 | float lastleft = 0; |
| narshu | 0:e238496b8073 | 139 | float lastright = 0; |
| narshu | 0:e238496b8073 | 140 | |
| narshu | 0:e238496b8073 | 141 | while (1) { |
| narshu | 0:e238496b8073 | 142 | Thread::signal_wait(0x1); |
| narshu | 0:e238496b8073 | 143 | OLED1 = !OLED1; |
| narshu | 0:e238496b8073 | 144 | |
| narshu | 0:e238496b8073 | 145 | int leftenc = motors.getEncoder1(); |
| narshu | 0:e238496b8073 | 146 | int rightenc = motors.getEncoder2(); |
| narshu | 0:e238496b8073 | 147 | |
| narshu | 0:e238496b8073 | 148 | float dleft = motors.encoderToDistance(leftenc-lastleft)/1000.0f; |
| narshu | 0:e238496b8073 | 149 | float dright = motors.encoderToDistance(rightenc-lastright)/1000.0f; |
| narshu | 0:e238496b8073 | 150 | |
| narshu | 0:e238496b8073 | 151 | lastleft = leftenc; |
| narshu | 0:e238496b8073 | 152 | lastright = rightenc; |
| narshu | 0:e238496b8073 | 153 | |
| narshu | 0:e238496b8073 | 154 | |
| narshu | 0:e238496b8073 | 155 | //The below calculation are in body frame (where +x is forward) |
| narshu | 0:e238496b8073 | 156 | float dxp, dyp,d,r; |
| narshu | 0:e238496b8073 | 157 | float thetap = (dright - dleft)*PI / (float(robotCircumference)/1000.0f); |
| narshu | 0:e238496b8073 | 158 | 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:e238496b8073 | 159 | d = (dright + dleft)/2.0f; |
| narshu | 0:e238496b8073 | 160 | dxp = d*cos(thetap/2.0f); |
| narshu | 0:e238496b8073 | 161 | dyp = d*sin(thetap/2.0f); |
| narshu | 0:e238496b8073 | 162 | |
| narshu | 0:e238496b8073 | 163 | } else { //calculate circle arc |
| narshu | 0:e238496b8073 | 164 | //float r = (right + left) / (4.0f * PI * thetap); |
| narshu | 0:e238496b8073 | 165 | r = (dright + dleft) / (2.0f*thetap); |
| narshu | 0:e238496b8073 | 166 | dxp = abs(r)*sin(thetap); |
| narshu | 0:e238496b8073 | 167 | dyp = r - r*cos(thetap); |
| narshu | 0:e238496b8073 | 168 | } |
| narshu | 0:e238496b8073 | 169 | |
| narshu | 0:e238496b8073 | 170 | statelock.lock(); |
| narshu | 0:e238496b8073 | 171 | |
| narshu | 0:e238496b8073 | 172 | //rotating to cartesian frame and updating state |
| narshu | 0:e238496b8073 | 173 | X(0) += dxp * cos(X(2)) - dyp * sin(X(2)); |
| narshu | 0:e238496b8073 | 174 | X(1) += dxp * sin(X(2)) + dyp * cos(X(2)); |
| narshu | 0:e238496b8073 | 175 | X(2) = rectifyAng(X(2) + thetap); |
| narshu | 0:e238496b8073 | 176 | |
| narshu | 0:e238496b8073 | 177 | //Linearising F around X |
| narshu | 0:e238496b8073 | 178 | Matrix<float, 3, 3> F; |
| narshu | 0:e238496b8073 | 179 | F = 1, 0, (dxp * -sin(X(2)) - dyp * cos(X(2))), |
| narshu | 0:e238496b8073 | 180 | 0, 1, (dxp * cos(X(2)) - dyp * sin(X(2))), |
| narshu | 0:e238496b8073 | 181 | 0, 0, 1; |
| narshu | 0:e238496b8073 | 182 | |
| narshu | 0:e238496b8073 | 183 | //Generating forward and rotational variance |
| narshu | 0:e238496b8073 | 184 | float varfwd = fwdvarperunit * (dright + dleft) / 2.0f; |
| narshu | 0:e238496b8073 | 185 | float varang = varperang * thetap; |
| narshu | 0:e238496b8073 | 186 | float varxydt = xyvarpertime * PREDICTPERIOD; |
| narshu | 0:e238496b8073 | 187 | float varangdt = angvarpertime * PREDICTPERIOD; |
| narshu | 0:e238496b8073 | 188 | |
| narshu | 0:e238496b8073 | 189 | //Rotating into cartesian frame |
| narshu | 0:e238496b8073 | 190 | Matrix<float, 2, 2> Qsub,Qsubrot,Qrot; |
| narshu | 0:e238496b8073 | 191 | Qsub = varfwd + varxydt, 0, |
| narshu | 0:e238496b8073 | 192 | 0, varxydt; |
| narshu | 0:e238496b8073 | 193 | |
| narshu | 0:e238496b8073 | 194 | Qrot = Rotmatrix(X(2)); |
| narshu | 0:e238496b8073 | 195 | |
| narshu | 0:e238496b8073 | 196 | Qsubrot = Qrot * Qsub * trans(Qrot); |
| narshu | 0:e238496b8073 | 197 | |
| narshu | 0:e238496b8073 | 198 | //Generate Q |
| narshu | 0:e238496b8073 | 199 | Matrix<float, 3, 3> Q;//(Qsubrot); |
| narshu | 0:e238496b8073 | 200 | Q = Qsubrot(0,0), Qsubrot(0,1), 0, |
| narshu | 0:e238496b8073 | 201 | Qsubrot(1,0), Qsubrot(1,1), 0, |
| narshu | 0:e238496b8073 | 202 | 0, 0, varang + varangdt; |
| narshu | 0:e238496b8073 | 203 | |
| narshu | 0:e238496b8073 | 204 | P = F * P * trans(F) + Q; |
| narshu | 0:e238496b8073 | 205 | |
| narshu | 0:e238496b8073 | 206 | statelock.unlock(); |
| narshu | 0:e238496b8073 | 207 | //Thread::wait(PREDICTPERIOD); |
| narshu | 0:e238496b8073 | 208 | |
| narshu | 0:e238496b8073 | 209 | //cout << "predict" << X << endl; |
| narshu | 0:e238496b8073 | 210 | //cout << P << endl; |
| narshu | 0:e238496b8073 | 211 | } |
| narshu | 0:e238496b8073 | 212 | } |
| narshu | 0:e238496b8073 | 213 | |
| narshu | 0:e238496b8073 | 214 | //void Kalman::sonarloop() { |
| narshu | 0:e238496b8073 | 215 | // while (1) { |
| narshu | 0:e238496b8073 | 216 | // Thread::signal_wait(0x1); |
| narshu | 0:e238496b8073 | 217 | // sonararray.startRange(); |
| narshu | 0:e238496b8073 | 218 | // } |
| narshu | 0:e238496b8073 | 219 | //} |
| narshu | 0:e238496b8073 | 220 | |
| narshu | 0:e238496b8073 | 221 | |
| narshu | 0:e238496b8073 | 222 | void Kalman::runupdate(measurement_t type, float value, float variance) { |
| narshu | 0:e238496b8073 | 223 | //printf("beacon %d dist %f\r\n", sonarid, dist); |
| narshu | 0:e238496b8073 | 224 | //led2 = !led2; |
| narshu | 0:e238496b8073 | 225 | |
| narshu | 0:e238496b8073 | 226 | measurmentdata* measured = (measurmentdata*)measureMQ.alloc(); |
| narshu | 0:e238496b8073 | 227 | if (measured) { |
| narshu | 0:e238496b8073 | 228 | measured->mtype = type; |
| narshu | 0:e238496b8073 | 229 | measured->value = value; |
| narshu | 0:e238496b8073 | 230 | measured->variance = variance; |
| narshu | 0:e238496b8073 | 231 | |
| narshu | 0:e238496b8073 | 232 | osStatus putret = measureMQ.put(measured); |
| narshu | 0:e238496b8073 | 233 | if (putret) |
| narshu | 0:e238496b8073 | 234 | OLED4 = 1; |
| narshu | 0:e238496b8073 | 235 | // printf("putting in MQ error code %#x\r\n", putret); |
| narshu | 0:e238496b8073 | 236 | } else { |
| narshu | 0:e238496b8073 | 237 | OLED4 = 1; |
| narshu | 0:e238496b8073 | 238 | //printf("MQalloc returned NULL ptr\r\n"); |
| narshu | 0:e238496b8073 | 239 | } |
| narshu | 0:e238496b8073 | 240 | |
| narshu | 0:e238496b8073 | 241 | } |
| narshu | 0:e238496b8073 | 242 | |
| narshu | 0:e238496b8073 | 243 | void Kalman::updateloop() { |
| narshu | 0:e238496b8073 | 244 | measurement_t type; |
| narshu | 0:e238496b8073 | 245 | float value,variance,rbx,rby,expecdist,Y; |
| narshu | 0:e238496b8073 | 246 | float dhdx,dhdy; |
| narshu | 0:e238496b8073 | 247 | bool aborton2stddev = false; |
| narshu | 0:e238496b8073 | 248 | |
| narshu | 0:e238496b8073 | 249 | Matrix<float, 1, 3> H; |
| narshu | 0:e238496b8073 | 250 | |
| narshu | 0:e238496b8073 | 251 | float S; |
| narshu | 0:e238496b8073 | 252 | Matrix<float, 3, 3> I3( identity< Matrix<float, 3, 3> >() ); |
| narshu | 0:e238496b8073 | 253 | |
| narshu | 0:e238496b8073 | 254 | |
| narshu | 0:e238496b8073 | 255 | while (1) { |
| narshu | 0:e238496b8073 | 256 | OLED2 = !OLED2; |
| narshu | 0:e238496b8073 | 257 | |
| narshu | 0:e238496b8073 | 258 | osEvent evt = measureMQ.get(); |
| narshu | 0:e238496b8073 | 259 | |
| narshu | 0:e238496b8073 | 260 | if (evt.status == osEventMail) { |
| narshu | 0:e238496b8073 | 261 | |
| narshu | 0:e238496b8073 | 262 | measurmentdata &measured = *(measurmentdata*)evt.value.p; |
| narshu | 0:e238496b8073 | 263 | type = measured.mtype; //Note, may support more measurment types than sonar in the future! |
| narshu | 0:e238496b8073 | 264 | value = measured.value; |
| narshu | 0:e238496b8073 | 265 | variance = measured.variance; |
| narshu | 0:e238496b8073 | 266 | |
| narshu | 0:e238496b8073 | 267 | // don't forget to free the memory |
| narshu | 0:e238496b8073 | 268 | measureMQ.free(&measured); |
| narshu | 0:e238496b8073 | 269 | |
| narshu | 0:e238496b8073 | 270 | if (type <= maxmeasure) { |
| narshu | 0:e238496b8073 | 271 | |
| narshu | 0:e238496b8073 | 272 | if (type <= SONAR3) { |
| narshu | 0:e238496b8073 | 273 | |
| narshu | 0:e238496b8073 | 274 | float dist = value / 1000.0f; //converting to m from mm |
| narshu | 0:e238496b8073 | 275 | int sonarid = type; |
| narshu | 0:e238496b8073 | 276 | aborton2stddev = false; |
| narshu | 0:e238496b8073 | 277 | |
| narshu | 0:e238496b8073 | 278 | // Remove the offset if possible |
| narshu | 0:e238496b8073 | 279 | if (Kalman_init) |
| narshu | 0:e238496b8073 | 280 | dist = dist - SonarMeasure_Offset[sonarid]; |
| narshu | 0:e238496b8073 | 281 | |
| narshu | 0:e238496b8073 | 282 | statelock.lock(); |
| narshu | 0:e238496b8073 | 283 | //update the current sonar readings |
| narshu | 0:e238496b8073 | 284 | SonarMeasures[sonarid] = dist; |
| narshu | 0:e238496b8073 | 285 | |
| narshu | 0:e238496b8073 | 286 | rbx = X(0) - beaconpos[sonarid].x/1000.0f; |
| narshu | 0:e238496b8073 | 287 | rby = X(1) - beaconpos[sonarid].y/1000.0f; |
| narshu | 0:e238496b8073 | 288 | |
| narshu | 0:e238496b8073 | 289 | expecdist = hypot(rbx, rby);//sqrt(rbx*rbx + rby*rby); |
| narshu | 0:e238496b8073 | 290 | Y = dist - expecdist; |
| narshu | 0:e238496b8073 | 291 | |
| narshu | 0:e238496b8073 | 292 | dhdx = rbx / expecdist; |
| narshu | 0:e238496b8073 | 293 | dhdy = rby / expecdist; |
| narshu | 0:e238496b8073 | 294 | |
| narshu | 0:e238496b8073 | 295 | H = dhdx, dhdy, 0; |
| narshu | 0:e238496b8073 | 296 | |
| narshu | 0:e238496b8073 | 297 | } else if (type <= IR3) { |
| narshu | 0:e238496b8073 | 298 | |
| narshu | 0:e238496b8073 | 299 | aborton2stddev = false; |
| narshu | 0:e238496b8073 | 300 | int IRidx = type-3; |
| narshu | 0:e238496b8073 | 301 | |
| narshu | 0:e238496b8073 | 302 | statelock.lock(); |
| narshu | 0:e238496b8073 | 303 | IRMeasures[IRidx] = value; |
| narshu | 0:e238496b8073 | 304 | |
| narshu | 0:e238496b8073 | 305 | rbx = X(0) - beaconpos[IRidx].x/1000.0f; |
| narshu | 0:e238496b8073 | 306 | rby = X(1) - beaconpos[IRidx].y/1000.0f; |
| narshu | 0:e238496b8073 | 307 | |
| narshu | 0:e238496b8073 | 308 | float expecang = atan2(-rby, -rbx) - X(2); |
| narshu | 0:e238496b8073 | 309 | Y = rectifyAng(value - expecang); |
| narshu | 0:e238496b8073 | 310 | |
| narshu | 0:e238496b8073 | 311 | float dstsq = rbx*rbx + rby*rby; |
| narshu | 0:e238496b8073 | 312 | H = -rby/dstsq, rbx/dstsq, -1; |
| narshu | 0:e238496b8073 | 313 | } |
| narshu | 0:e238496b8073 | 314 | |
| narshu | 0:e238496b8073 | 315 | Matrix<float, 3, 1> PH (P * trans(H)); |
| narshu | 0:e238496b8073 | 316 | S = (H * PH)(0,0) + variance; |
| narshu | 0:e238496b8073 | 317 | |
| narshu | 0:e238496b8073 | 318 | if (aborton2stddev && Y*Y > 4 * S) { |
| narshu | 0:e238496b8073 | 319 | statelock.unlock(); |
| narshu | 0:e238496b8073 | 320 | continue; |
| narshu | 0:e238496b8073 | 321 | } |
| narshu | 0:e238496b8073 | 322 | |
| narshu | 0:e238496b8073 | 323 | Matrix<float, 3, 1> K (PH * (1/S)); |
| narshu | 0:e238496b8073 | 324 | |
| narshu | 0:e238496b8073 | 325 | //Updating state |
| narshu | 0:e238496b8073 | 326 | X += col(K, 0) * Y; |
| narshu | 0:e238496b8073 | 327 | X(2) = rectifyAng(X(2)); |
| narshu | 0:e238496b8073 | 328 | |
| narshu | 0:e238496b8073 | 329 | P = (I3 - K * H) * P; |
| narshu | 0:e238496b8073 | 330 | |
| narshu | 0:e238496b8073 | 331 | statelock.unlock(); |
| narshu | 0:e238496b8073 | 332 | |
| narshu | 0:e238496b8073 | 333 | } |
| narshu | 0:e238496b8073 | 334 | |
| narshu | 0:e238496b8073 | 335 | } else { |
| narshu | 0:e238496b8073 | 336 | OLED4 = 1; |
| narshu | 0:e238496b8073 | 337 | //printf("ERROR: in updateloop, code %#x", evt); |
| narshu | 0:e238496b8073 | 338 | } |
| narshu | 0:e238496b8073 | 339 | |
| narshu | 0:e238496b8073 | 340 | } |
| narshu | 0:e238496b8073 | 341 | |
| narshu | 0:e238496b8073 | 342 | } |