Ryan Savitski / Mbed 2 deprecated ICRSEurobot14

2014 Eurobot fork

Dependencies:   mbed-rtos mbed QEI

Processes/Kalman/Kalman.cpp@27:664e81033846, 2013-04-10 (annotated)

Committer:
madcowswe
Date:
Wed Apr 10 03:48:42 2013 +0000
Revision:
27:664e81033846
Parent:
26:7cb3a21d9a2e
Child:
29:00e1493b44f0
minor opti in update loop

Who changed what in which revision?

UserRevisionLine numberNew contents of line
madcowswe 16:52250d8d8fce 1 //***************************************************************************************
madcowswe 16:52250d8d8fce 2 //Kalman Filter implementation
madcowswe 16:52250d8d8fce 3 //***************************************************************************************
madcowswe 16:52250d8d8fce 4 #include "Kalman.h"
madcowswe 16:52250d8d8fce 5 #include "rtos.h"
madcowswe 16:52250d8d8fce 6 #include "math.h"
madcowswe 16:52250d8d8fce 7 #include "supportfuncs.h"
madcowswe 20:70d651156779 8 #include "Encoder.h"
madcowswe 21:167dacfe0b14 9 #include "globals.h"
madcowswe 21:167dacfe0b14 10 #include "Printing.h"
madcowswe 16:52250d8d8fce 11
madcowswe 20:70d651156779 12 #include "tvmet/Matrix.h"
madcowswe 16:52250d8d8fce 13 using namespace tvmet;
madcowswe 16:52250d8d8fce 14
madcowswe 16:52250d8d8fce 15
madcowswe 16:52250d8d8fce 16
madcowswe 16:52250d8d8fce 17 namespace Kalman
madcowswe 16:52250d8d8fce 18 {
madcowswe 16:52250d8d8fce 19
madcowswe 20:70d651156779 20 Ticker predictticker;
madcowswe 20:70d651156779 21
madcowswe 20:70d651156779 22 DigitalOut OLED4(LED4);
madcowswe 20:70d651156779 23 DigitalOut OLED1(LED1);
madcowswe 26:7cb3a21d9a2e 24 DigitalOut OLED2(LED2);
madcowswe 20:70d651156779 25
madcowswe 16:52250d8d8fce 26 //State variables
madcowswe 19:4b993a9a156e 27 Matrix<float, 3, 1> X;
madcowswe 16:52250d8d8fce 28 Matrix<float, 3, 3> P;
madcowswe 16:52250d8d8fce 29 Mutex statelock;
madcowswe 16:52250d8d8fce 30
madcowswe 16:52250d8d8fce 31 float RawReadings[maxmeasure+1];
madcowswe 22:6e3218cf75f8 32 float IRphaseOffset;
madcowswe 16:52250d8d8fce 33
madcowswe 20:70d651156779 34 bool Kalman_inited = 0;
madcowswe 16:52250d8d8fce 35
madcowswe 16:52250d8d8fce 36 struct measurmentdata {
madcowswe 16:52250d8d8fce 37 measurement_t mtype;
madcowswe 16:52250d8d8fce 38 float value;
madcowswe 16:52250d8d8fce 39 float variance;
madcowswe 19:4b993a9a156e 40 };
madcowswe 16:52250d8d8fce 41
madcowswe 16:52250d8d8fce 42 Mail <measurmentdata, 16> measureMQ;
madcowswe 16:52250d8d8fce 43
madcowswe 20:70d651156779 44 Thread* predict_thread_ptr = NULL;
madcowswe 16:52250d8d8fce 45
madcowswe 16:52250d8d8fce 46
madcowswe 16:52250d8d8fce 47 //Note: this init function assumes that the robot faces east, theta=0, in the +x direction
madcowswe 16:52250d8d8fce 48 void KalmanInit()
madcowswe 16:52250d8d8fce 49 {
madcowswe 20:70d651156779 50 printf("kalmaninit \r\n");
madcowswe 20:70d651156779 51
madcowswe 20:70d651156779 52 //WARNING: HARDCODED!
madcowswe 20:70d651156779 53
madcowswe 16:52250d8d8fce 54 //solve for our position (assume perfect bias)
madcowswe 20:70d651156779 55 const float d = beaconpos[2].y - beaconpos[1].y;
madcowswe 20:70d651156779 56 const float i = beaconpos[2].y - beaconpos[0].y;
madcowswe 20:70d651156779 57 const float j = beaconpos[2].x - beaconpos[0].x;
madcowswe 20:70d651156779 58 float r1 = RawReadings[SONAR2];
madcowswe 19:4b993a9a156e 59 float r2 = RawReadings[SONAR1];
madcowswe 20:70d651156779 60 float r3 = RawReadings[SONAR0];
madcowswe 20:70d651156779 61
madcowswe 20:70d651156779 62 printf("ranges: 0: %0.4f, 1: %0.4f, 2: %0.4f \r\n", r1, r2, r3);
madcowswe 17:6263e90bf3ba 63
madcowswe 19:4b993a9a156e 64 float y_coor = (r1*r1-r2*r2+d*d)/(2*d);
madcowswe 17:6263e90bf3ba 65 float x_coor = (r1*r1-r3*r3+i*i+j*j)/(2*j) - (i*y_coor)/j;
madcowswe 20:70d651156779 66
madcowswe 20:70d651156779 67 //coordinate system hack (for now)
madcowswe 20:70d651156779 68 x_coor = beaconpos[2].x - x_coor;
madcowswe 20:70d651156779 69 y_coor = beaconpos[2].y - y_coor;
madcowswe 20:70d651156779 70
madcowswe 20:70d651156779 71 printf("solved pos from sonar: %f, %f \r\n", x_coor, y_coor);
madcowswe 20:70d651156779 72
madcowswe 16:52250d8d8fce 73 //IR
madcowswe 16:52250d8d8fce 74 float IRMeasuresloc[3];
madcowswe 16:52250d8d8fce 75 IRMeasuresloc[0] = RawReadings[IR0];
madcowswe 16:52250d8d8fce 76 IRMeasuresloc[1] = RawReadings[IR1];
madcowswe 16:52250d8d8fce 77 IRMeasuresloc[2] = RawReadings[IR2];
madcowswe 20:70d651156779 78 printf("IR 0: %0.4f, 1: %0.4f, 2: %0.4f \r\n", IRMeasuresloc[0]*180/PI, IRMeasuresloc[1]*180/PI, IRMeasuresloc[2]*180/PI);
madcowswe 16:52250d8d8fce 79
madcowswe 17:6263e90bf3ba 80 float IR_Offsets[3];
madcowswe 17:6263e90bf3ba 81 float fromb0offset = 0;
madcowswe 16:52250d8d8fce 82 for (int i = 0; i < 3; i++) {
madcowswe 16:52250d8d8fce 83
madcowswe 16:52250d8d8fce 84 //Compute IR offset
madcowswe 16:52250d8d8fce 85 float angle_est = atan2(beaconpos[i].y - y_coor,beaconpos[i].x - x_coor);
madcowswe 17:6263e90bf3ba 86
madcowswe 16:52250d8d8fce 87 //printf("Angle %d : %f \n\r",i,angle_est*180/PI );
madcowswe 19:4b993a9a156e 88 IR_Offsets[i] = constrainAngle(IRMeasuresloc[i] - angle_est);
madcowswe 20:70d651156779 89
madcowswe 19:4b993a9a156e 90 fromb0offset += constrainAngle(IR_Offsets[i] - IR_Offsets[0]);
madcowswe 16:52250d8d8fce 91 }
madcowswe 20:70d651156779 92
madcowswe 22:6e3218cf75f8 93 IRphaseOffset = constrainAngle(IR_Offsets[0] + fromb0offset/3);
madcowswe 16:52250d8d8fce 94
madcowswe 16:52250d8d8fce 95 //debug
madcowswe 22:6e3218cf75f8 96 printf("Offsets IR: %0.4f\r\n",IRphaseOffset*180/PI);
madcowswe 16:52250d8d8fce 97
madcowswe 16:52250d8d8fce 98 statelock.lock();
madcowswe 26:7cb3a21d9a2e 99 X(0,0) = x_coor-TURRET_FWD_PLACEMENT; //assume facing east
madcowswe 19:4b993a9a156e 100 X(1,0) = y_coor;
madcowswe 19:4b993a9a156e 101 X(2,0) = 0;
madcowswe 21:167dacfe0b14 102
madcowswe 21:167dacfe0b14 103 P = 0.02*0.02, 0, 0,
madcowswe 21:167dacfe0b14 104 0, 0.02*0.02, 0,
madcowswe 21:167dacfe0b14 105 0, 0, 0.04;
madcowswe 16:52250d8d8fce 106 statelock.unlock();
madcowswe 20:70d651156779 107
madcowswe 20:70d651156779 108 Kalman_inited = 1;
madcowswe 16:52250d8d8fce 109 }
madcowswe 16:52250d8d8fce 110
madcowswe 20:70d651156779 111
madcowswe 20:70d651156779 112 State getState(){
madcowswe 20:70d651156779 113 statelock.lock();
madcowswe 20:70d651156779 114 State state = {X(0,0), X(1,0), X(2,0)};
madcowswe 20:70d651156779 115 statelock.unlock();
madcowswe 20:70d651156779 116 return state;
madcowswe 20:70d651156779 117 }
madcowswe 20:70d651156779 118
madcowswe 20:70d651156779 119
madcowswe 21:167dacfe0b14 120 void predictloop(void const*)
madcowswe 16:52250d8d8fce 121 {
madcowswe 16:52250d8d8fce 122
madcowswe 21:167dacfe0b14 123 OLED4 = !Printing::registerID(0, 3);
madcowswe 21:167dacfe0b14 124 OLED4 = !Printing::registerID(1, 4);
madcowswe 16:52250d8d8fce 125
madcowswe 16:52250d8d8fce 126 float lastleft = 0;
madcowswe 16:52250d8d8fce 127 float lastright = 0;
madcowswe 16:52250d8d8fce 128
madcowswe 16:52250d8d8fce 129 while (1) {
madcowswe 16:52250d8d8fce 130 Thread::signal_wait(0x1);
madcowswe 16:52250d8d8fce 131 OLED1 = !OLED1;
madcowswe 16:52250d8d8fce 132
madcowswe 20:70d651156779 133 float leftenc = left_encoder.getTicks() * ENCODER_M_PER_TICK;
madcowswe 20:70d651156779 134 float rightenc = right_encoder.getTicks() * ENCODER_M_PER_TICK;
madcowswe 16:52250d8d8fce 135
madcowswe 20:70d651156779 136 float dleft = leftenc-lastleft;
madcowswe 20:70d651156779 137 float dright = rightenc-lastright;
madcowswe 16:52250d8d8fce 138
madcowswe 16:52250d8d8fce 139 lastleft = leftenc;
madcowswe 16:52250d8d8fce 140 lastright = rightenc;
madcowswe 16:52250d8d8fce 141
madcowswe 16:52250d8d8fce 142
madcowswe 16:52250d8d8fce 143 //The below calculation are in body frame (where +x is forward)
madcowswe 16:52250d8d8fce 144 float dxp, dyp,d,r;
madcowswe 20:70d651156779 145 float thetap = (dright - dleft) / ENCODER_WHEELBASE;
madcowswe 20:70d651156779 146 if (abs(thetap) < 0.01f) { //if the rotation through the integration step is small, approximate with a straight line to avoid numerical error
madcowswe 16:52250d8d8fce 147 d = (dright + dleft)/2.0f;
madcowswe 16:52250d8d8fce 148 dxp = d*cos(thetap/2.0f);
madcowswe 16:52250d8d8fce 149 dyp = d*sin(thetap/2.0f);
madcowswe 16:52250d8d8fce 150
madcowswe 16:52250d8d8fce 151 } else { //calculate circle arc
madcowswe 16:52250d8d8fce 152 //float r = (right + left) / (4.0f * PI * thetap);
madcowswe 16:52250d8d8fce 153 r = (dright + dleft) / (2.0f*thetap);
madcowswe 20:70d651156779 154 dxp = r*sin(thetap);
madcowswe 16:52250d8d8fce 155 dyp = r - r*cos(thetap);
madcowswe 16:52250d8d8fce 156 }
madcowswe 16:52250d8d8fce 157
madcowswe 16:52250d8d8fce 158 statelock.lock();
madcowswe 16:52250d8d8fce 159
madcowswe 20:70d651156779 160 float tempX2 = X(2,0);
madcowswe 16:52250d8d8fce 161 //rotating to cartesian frame and updating state
madcowswe 20:70d651156779 162 X(0,0) += dxp * cos(X(2,0)) - dyp * sin(X(2,0));
madcowswe 20:70d651156779 163 X(1,0) += dxp * sin(X(2,0)) + dyp * cos(X(2,0));
madcowswe 20:70d651156779 164 X(2,0) = constrainAngle(X(2,0) + thetap);
madcowswe 16:52250d8d8fce 165
madcowswe 16:52250d8d8fce 166 //Linearising F around X
madcowswe 20:70d651156779 167 float avgX2 = (X(2,0) + tempX2)/2.0f;
madcowswe 16:52250d8d8fce 168 Matrix<float, 3, 3> F;
madcowswe 16:52250d8d8fce 169 F = 1, 0, (dxp * -sin(avgX2) - dyp * cos(avgX2)),
madcowswe 16:52250d8d8fce 170 0, 1, (dxp * cos(avgX2) - dyp * sin(avgX2)),
madcowswe 16:52250d8d8fce 171 0, 0, 1;
madcowswe 16:52250d8d8fce 172
madcowswe 16:52250d8d8fce 173 //Generating forward and rotational variance
madcowswe 16:52250d8d8fce 174 float varfwd = fwdvarperunit * abs(dright + dleft) / 2.0f;
madcowswe 16:52250d8d8fce 175 float varang = varperang * abs(thetap);
madcowswe 20:70d651156779 176 float varxydt = xyvarpertime * KALMAN_PREDICT_PERIOD;
madcowswe 20:70d651156779 177 float varangdt = angvarpertime * KALMAN_PREDICT_PERIOD;
madcowswe 16:52250d8d8fce 178
madcowswe 16:52250d8d8fce 179 //Rotating into cartesian frame
madcowswe 16:52250d8d8fce 180 Matrix<float, 2, 2> Qsub,Qsubrot,Qrot;
madcowswe 16:52250d8d8fce 181 Qsub = varfwd + varxydt, 0,
madcowswe 16:52250d8d8fce 182 0, varxydt;
madcowswe 16:52250d8d8fce 183
madcowswe 20:70d651156779 184 Qrot = Rotmatrix(X(2,0));
madcowswe 16:52250d8d8fce 185
madcowswe 16:52250d8d8fce 186 Qsubrot = Qrot * Qsub * trans(Qrot);
madcowswe 16:52250d8d8fce 187
madcowswe 16:52250d8d8fce 188 //Generate Q
madcowswe 16:52250d8d8fce 189 Matrix<float, 3, 3> Q;//(Qsubrot);
madcowswe 16:52250d8d8fce 190 Q = Qsubrot(0,0), Qsubrot(0,1), 0,
madcowswe 16:52250d8d8fce 191 Qsubrot(1,0), Qsubrot(1,1), 0,
madcowswe 16:52250d8d8fce 192 0, 0, varang + varangdt;
madcowswe 16:52250d8d8fce 193
madcowswe 16:52250d8d8fce 194 P = F * P * trans(F) + Q;
madcowswe 16:52250d8d8fce 195
madcowswe 20:70d651156779 196 //printf("x: %f, y: %f, t: %f\r\n", X(0,0), X(1,0), X(2,0));
madcowswe 21:167dacfe0b14 197 //Update Printing
madcowswe 21:167dacfe0b14 198 float statecpy[] = {X(0,0), X(1,0), X(2,0)};
madcowswe 21:167dacfe0b14 199 Printing::updateval(0, statecpy, 3);
madcowswe 16:52250d8d8fce 200
madcowswe 21:167dacfe0b14 201 float Pcpy[] = {P(0,0), P(0,1), P(1,0), P(1,1)};
madcowswe 21:167dacfe0b14 202 Printing::updateval(1, Pcpy, 4);
madcowswe 16:52250d8d8fce 203
madcowswe 16:52250d8d8fce 204 statelock.unlock();
madcowswe 16:52250d8d8fce 205 }
madcowswe 16:52250d8d8fce 206 }
madcowswe 16:52250d8d8fce 207
madcowswe 20:70d651156779 208
madcowswe 20:70d651156779 209 void predict_event_setter(){
madcowswe 20:70d651156779 210 if(predict_thread_ptr)
madcowswe 20:70d651156779 211 predict_thread_ptr->signal_set(0x1);
madcowswe 20:70d651156779 212 else
madcowswe 20:70d651156779 213 OLED4 = 1;
madcowswe 20:70d651156779 214 }
madcowswe 20:70d651156779 215
madcowswe 20:70d651156779 216 void start_predict_ticker(Thread* predict_thread_ptr_in){
madcowswe 20:70d651156779 217 predict_thread_ptr = predict_thread_ptr_in;
madcowswe 20:70d651156779 218 predictticker.attach(predict_event_setter, KALMAN_PREDICT_PERIOD);
madcowswe 20:70d651156779 219 }
madcowswe 20:70d651156779 220
madcowswe 20:70d651156779 221 void runupdate(measurement_t type, float value, float variance)
madcowswe 16:52250d8d8fce 222 {
madcowswe 20:70d651156779 223 if (!Kalman_inited) {
madcowswe 16:52250d8d8fce 224 RawReadings[type] = value;
madcowswe 20:70d651156779 225 } else {
madcowswe 17:6263e90bf3ba 226
madcowswe 20:70d651156779 227 if (type >= IR0 && type <= IR2)
madcowswe 22:6e3218cf75f8 228 RawReadings[type] = value - IRphaseOffset;
madcowswe 20:70d651156779 229 else
madcowswe 20:70d651156779 230 RawReadings[type] = value;
madcowswe 20:70d651156779 231
madcowswe 17:6263e90bf3ba 232
madcowswe 16:52250d8d8fce 233 measurmentdata* measured = (measurmentdata*)measureMQ.alloc();
madcowswe 16:52250d8d8fce 234 if (measured) {
madcowswe 16:52250d8d8fce 235 measured->mtype = type;
madcowswe 16:52250d8d8fce 236 measured->value = value;
madcowswe 16:52250d8d8fce 237 measured->variance = variance;
madcowswe 16:52250d8d8fce 238
madcowswe 16:52250d8d8fce 239 osStatus putret = measureMQ.put(measured);
madcowswe 20:70d651156779 240 //if (putret)
madcowswe 20:70d651156779 241 //OLED4 = 1;
madcowswe 16:52250d8d8fce 242 // printf("putting in MQ error code %#x\r\n", putret);
madcowswe 16:52250d8d8fce 243 } else {
madcowswe 20:70d651156779 244 //OLED4 = 1;
madcowswe 16:52250d8d8fce 245 //printf("MQalloc returned NULL ptr\r\n");
madcowswe 16:52250d8d8fce 246 }
madcowswe 20:70d651156779 247
madcowswe 16:52250d8d8fce 248 }
madcowswe 20:70d651156779 249
madcowswe 16:52250d8d8fce 250
madcowswe 16:52250d8d8fce 251 }
madcowswe 26:7cb3a21d9a2e 252
madcowswe 21:167dacfe0b14 253 void Kalman::updateloop(void const*)
madcowswe 16:52250d8d8fce 254 {
madcowswe 16:52250d8d8fce 255
madcowswe 16:52250d8d8fce 256 //sonar Y chanels
madcowswe 26:7cb3a21d9a2e 257 OLED4 = !Printing::registerID(2, 1);
madcowswe 26:7cb3a21d9a2e 258 OLED4 = !Printing::registerID(3, 1);
madcowswe 26:7cb3a21d9a2e 259 OLED4 = !Printing::registerID(4, 1);
madcowswe 16:52250d8d8fce 260
madcowswe 16:52250d8d8fce 261 //IR Y chanels
madcowswe 26:7cb3a21d9a2e 262 OLED4 = !Printing::registerID(5, 1);
madcowswe 26:7cb3a21d9a2e 263 OLED4 = !Printing::registerID(6, 1);
madcowswe 26:7cb3a21d9a2e 264 OLED4 = !Printing::registerID(7, 1);
madcowswe 16:52250d8d8fce 265
madcowswe 16:52250d8d8fce 266 bool aborton2stddev = false;
madcowswe 16:52250d8d8fce 267
madcowswe 16:52250d8d8fce 268 Matrix<float, 1, 3> H;
madcowswe 16:52250d8d8fce 269
madcowswe 26:7cb3a21d9a2e 270 float Y,S;
madcowswe 26:7cb3a21d9a2e 271 const Matrix<float, 3, 3> I3( identity< Matrix<float, 3, 3> >() );
madcowswe 16:52250d8d8fce 272
madcowswe 16:52250d8d8fce 273
madcowswe 16:52250d8d8fce 274 while (1) {
madcowswe 16:52250d8d8fce 275 OLED2 = !OLED2;
madcowswe 16:52250d8d8fce 276
madcowswe 16:52250d8d8fce 277 osEvent evt = measureMQ.get();
madcowswe 16:52250d8d8fce 278
madcowswe 16:52250d8d8fce 279 if (evt.status == osEventMail) {
madcowswe 16:52250d8d8fce 280
madcowswe 16:52250d8d8fce 281 measurmentdata &measured = *(measurmentdata*)evt.value.p;
madcowswe 26:7cb3a21d9a2e 282 measurement_t type = measured.mtype; //Note, may support more measurment types than sonar in the future!
madcowswe 26:7cb3a21d9a2e 283 float value = measured.value;
madcowswe 26:7cb3a21d9a2e 284 float variance = measured.variance;
madcowswe 16:52250d8d8fce 285
madcowswe 16:52250d8d8fce 286 // don't forget to free the memory
madcowswe 16:52250d8d8fce 287 measureMQ.free(&measured);
madcowswe 16:52250d8d8fce 288
madcowswe 16:52250d8d8fce 289 if (type <= maxmeasure) {
madcowswe 16:52250d8d8fce 290
madcowswe 26:7cb3a21d9a2e 291 if (type <= SONAR2) {
madcowswe 16:52250d8d8fce 292
madcowswe 26:7cb3a21d9a2e 293 float dist = value;
madcowswe 16:52250d8d8fce 294 int sonarid = type;
madcowswe 16:52250d8d8fce 295 aborton2stddev = true;
madcowswe 16:52250d8d8fce 296
madcowswe 16:52250d8d8fce 297 statelock.lock();
madcowswe 26:7cb3a21d9a2e 298
madcowswe 26:7cb3a21d9a2e 299 float fp_ct = TURRET_FWD_PLACEMENT*cos(X(2,0));
madcowswe 26:7cb3a21d9a2e 300 float fp_st = TURRET_FWD_PLACEMENT*sin(X(2,0));
madcowswe 26:7cb3a21d9a2e 301
madcowswe 26:7cb3a21d9a2e 302 float rbx = X(0,0) + fp_ct - beaconpos[sonarid].x;
madcowswe 26:7cb3a21d9a2e 303 float rby = X(1,0) + fp_st - beaconpos[sonarid].y;
madcowswe 26:7cb3a21d9a2e 304
madcowswe 26:7cb3a21d9a2e 305 float expecdist = hypot(rbx, rby);//sqrt(rbx*rbx + rby*rby);
madcowswe 16:52250d8d8fce 306 Y = dist - expecdist;
madcowswe 16:52250d8d8fce 307
madcowswe 16:52250d8d8fce 308 //send to ui
madcowswe 26:7cb3a21d9a2e 309 Printing::updateval(sonarid+2, Y);
madcowswe 16:52250d8d8fce 310
madcowswe 26:7cb3a21d9a2e 311 float r_expecdist = 1.0f/expecdist;
madcowswe 16:52250d8d8fce 312
madcowswe 26:7cb3a21d9a2e 313 float dhdx = rbx * r_expecdist;
madcowswe 26:7cb3a21d9a2e 314 float dhdy = rby * r_expecdist;
madcowswe 27:664e81033846 315 float dhdt = fp_ct*dhdy - fp_st*dhdx;
madcowswe 16:52250d8d8fce 316
madcowswe 26:7cb3a21d9a2e 317 H = dhdx, dhdy, dhdt;
madcowswe 16:52250d8d8fce 318
madcowswe 26:7cb3a21d9a2e 319 } else if (type <= IR2) {
madcowswe 26:7cb3a21d9a2e 320
madcowswe 26:7cb3a21d9a2e 321 aborton2stddev = true;
madcowswe 16:52250d8d8fce 322 int IRidx = type-3;
madcowswe 16:52250d8d8fce 323
madcowswe 26:7cb3a21d9a2e 324 statelock.lock();
madcowswe 26:7cb3a21d9a2e 325
madcowswe 26:7cb3a21d9a2e 326 float fp_ct = TURRET_FWD_PLACEMENT*cos(X(2,0));
madcowswe 26:7cb3a21d9a2e 327 float fp_st = TURRET_FWD_PLACEMENT*sin(X(2,0));
madcowswe 16:52250d8d8fce 328
madcowswe 26:7cb3a21d9a2e 329 float brx = beaconpos[IRidx].x - (X(0,0) + fp_ct);
madcowswe 26:7cb3a21d9a2e 330 float bry = beaconpos[IRidx].y - (X(1,0) + fp_st);
madcowswe 16:52250d8d8fce 331
madcowswe 26:7cb3a21d9a2e 332 float expecang = atan2(bry, brx) - X(2,0); //constrainAngle can be called late
madcowswe 26:7cb3a21d9a2e 333 Y = constrainAngle(value - expecang);
madcowswe 16:52250d8d8fce 334
madcowswe 16:52250d8d8fce 335 //send to ui
madcowswe 26:7cb3a21d9a2e 336 Printing::updateval(IRidx + 5, Y);
madcowswe 16:52250d8d8fce 337
madcowswe 26:7cb3a21d9a2e 338 float r_dstsq = 1.0f/(brx*brx + bry*bry);
madcowswe 26:7cb3a21d9a2e 339 float dhdx = -bry*r_dstsq;
madcowswe 26:7cb3a21d9a2e 340 float dhdy = brx*r_dstsq;
madcowswe 26:7cb3a21d9a2e 341 float dhdt = fp_ct*dhdy - fp_st*dhdx - 1.0f;
madcowswe 26:7cb3a21d9a2e 342 H = dhdx, dhdy, dhdt;
madcowswe 16:52250d8d8fce 343 }
madcowswe 16:52250d8d8fce 344
madcowswe 16:52250d8d8fce 345 Matrix<float, 3, 1> PH (P * trans(H));
madcowswe 16:52250d8d8fce 346 S = (H * PH)(0,0) + variance;
madcowswe 16:52250d8d8fce 347
madcowswe 16:52250d8d8fce 348 if (aborton2stddev && Y*Y > 4 * S) {
madcowswe 16:52250d8d8fce 349 statelock.unlock();
madcowswe 16:52250d8d8fce 350 continue;
madcowswe 16:52250d8d8fce 351 }
madcowswe 16:52250d8d8fce 352
madcowswe 16:52250d8d8fce 353 Matrix<float, 3, 1> K (PH * (1/S));
madcowswe 16:52250d8d8fce 354
madcowswe 16:52250d8d8fce 355 //Updating state
madcowswe 26:7cb3a21d9a2e 356 X += K * Y;
madcowswe 26:7cb3a21d9a2e 357 X(2,0) = constrainAngle(X(2,0));
madcowswe 16:52250d8d8fce 358
madcowswe 16:52250d8d8fce 359 P = (I3 - K * H) * P;
madcowswe 16:52250d8d8fce 360
madcowswe 16:52250d8d8fce 361 statelock.unlock();
madcowswe 16:52250d8d8fce 362
madcowswe 16:52250d8d8fce 363 }
madcowswe 16:52250d8d8fce 364
madcowswe 16:52250d8d8fce 365 } else {
madcowswe 16:52250d8d8fce 366 OLED4 = 1;
madcowswe 16:52250d8d8fce 367 //printf("ERROR: in updateloop, code %#x", evt);
madcowswe 16:52250d8d8fce 368 }
madcowswe 16:52250d8d8fce 369
madcowswe 16:52250d8d8fce 370 }
madcowswe 16:52250d8d8fce 371
madcowswe 16:52250d8d8fce 372 }
madcowswe 16:52250d8d8fce 373
madcowswe 19:4b993a9a156e 374
madcowswe 16:52250d8d8fce 375 } //Kalman Namespace