Shuto Naruse / Mbed 2 deprecated Eurobot2012_Primary

Eurobot2012_Primary

Dependencies:   mbed Eurobot_2012_Primary

Kalman/Kalman.cpp@0:f3bf6c7e2283, 2012-04-20 (annotated)

Committer:
narshu
Date:
Fri Apr 20 20:39:35 2012 +0000
Revision:
0:f3bf6c7e2283
Child:
1:bbabbd997d21
inverted sonar echo input at pin14 and pin15

Who changed what in which revision?

UserRevisionLine numberNew contents of line
narshu 0:f3bf6c7e2283 1 //***************************************************************************************
narshu 0:f3bf6c7e2283 2 //Kalman Filter implementation
narshu 0:f3bf6c7e2283 3 //***************************************************************************************
narshu 0:f3bf6c7e2283 4 #include "Kalman.h"
narshu 0:f3bf6c7e2283 5 #include "rtos.h"
narshu 0:f3bf6c7e2283 6 #include "RFSRF05.h"
narshu 0:f3bf6c7e2283 7 //#include "MatrixMath.h"
narshu 0:f3bf6c7e2283 8 //#include "Matrix.h"
narshu 0:f3bf6c7e2283 9 #include "math.h"
narshu 0:f3bf6c7e2283 10 #include "globals.h"
narshu 0:f3bf6c7e2283 11 #include "motors.h"
narshu 0:f3bf6c7e2283 12 #include "system.h"
narshu 0:f3bf6c7e2283 13
narshu 0:f3bf6c7e2283 14 #include <tvmet/Matrix.h>
narshu 0:f3bf6c7e2283 15 #include <tvmet/Vector.h>
narshu 0:f3bf6c7e2283 16 using namespace tvmet;
narshu 0:f3bf6c7e2283 17 DigitalOut led1(LED1);
narshu 0:f3bf6c7e2283 18 DigitalOut led2(LED2);
narshu 0:f3bf6c7e2283 19 DigitalOut led3(LED3);
narshu 0:f3bf6c7e2283 20 DigitalOut led4(LED4);
narshu 0:f3bf6c7e2283 21
narshu 0:f3bf6c7e2283 22
narshu 0:f3bf6c7e2283 23 Kalman::Kalman(Motors &motorsin) :
narshu 0:f3bf6c7e2283 24 sonararray(p23,p14,p14,p14,p15,p15,p15,p5,p6,p7,p8,p11),
narshu 0:f3bf6c7e2283 25 motors(motorsin),
narshu 0:f3bf6c7e2283 26 predictthread(predictloopwrapper, this, osPriorityNormal, 512),
narshu 0:f3bf6c7e2283 27 predictticker( SIGTICKARGS(predictthread, 0x1) ),
narshu 0:f3bf6c7e2283 28 // sonarthread(sonarloopwrapper, this, osPriorityNormal, 256),
narshu 0:f3bf6c7e2283 29 // sonarticker( SIGTICKARGS(sonarthread, 0x1) ),
narshu 0:f3bf6c7e2283 30 updatethread(updateloopwrapper, this, osPriorityNormal, 512) {
narshu 0:f3bf6c7e2283 31
narshu 0:f3bf6c7e2283 32 //Initilising matrices
narshu 0:f3bf6c7e2283 33
narshu 0:f3bf6c7e2283 34 // X = x, y, theta;
narshu 0:f3bf6c7e2283 35 X = 0.5, 0, 0;
narshu 0:f3bf6c7e2283 36
narshu 0:f3bf6c7e2283 37 P = 1, 0, 0,
narshu 0:f3bf6c7e2283 38 0, 1, 0,
narshu 0:f3bf6c7e2283 39 0, 0, 0.04;
narshu 0:f3bf6c7e2283 40
narshu 0:f3bf6c7e2283 41 Q = 0.002, 0, 0, //temporary matrix, Use dt!
narshu 0:f3bf6c7e2283 42 0, 0.002, 0,
narshu 0:f3bf6c7e2283 43 0, 0, 0.002;
narshu 0:f3bf6c7e2283 44
narshu 0:f3bf6c7e2283 45 //measurment variance R is provided by each sensor when calling runupdate
narshu 0:f3bf6c7e2283 46
narshu 0:f3bf6c7e2283 47 //attach callback
narshu 0:f3bf6c7e2283 48 sonararray.callbackobj = (DummyCT*)this;
narshu 0:f3bf6c7e2283 49 sonararray.mcallbackfunc = (void (DummyCT::*)(int beaconnum, float distance, float variance)) &Kalman::runupdate;
narshu 0:f3bf6c7e2283 50
narshu 0:f3bf6c7e2283 51
narshu 0:f3bf6c7e2283 52 // predictticker.start(20);
narshu 0:f3bf6c7e2283 53 // sonarticker.start(50);
narshu 0:f3bf6c7e2283 54
narshu 0:f3bf6c7e2283 55
narshu 0:f3bf6c7e2283 56 }
narshu 0:f3bf6c7e2283 57
narshu 0:f3bf6c7e2283 58
narshu 0:f3bf6c7e2283 59 void Kalman::predictloop() {
narshu 0:f3bf6c7e2283 60 Matrix<float, 3, 3> F;
narshu 0:f3bf6c7e2283 61 int lastleft = motors.getEncoder1();
narshu 0:f3bf6c7e2283 62 int lastright = motors.getEncoder2();
narshu 0:f3bf6c7e2283 63 int leftenc;
narshu 0:f3bf6c7e2283 64 int rightenc;
narshu 0:f3bf6c7e2283 65 float dleft,dright;
narshu 0:f3bf6c7e2283 66 float dxp, dyp;
narshu 0:f3bf6c7e2283 67 float thetap,d,r;
narshu 0:f3bf6c7e2283 68
narshu 0:f3bf6c7e2283 69 while (1) {
narshu 0:f3bf6c7e2283 70 // Thread::signal_wait(0x1);
narshu 0:f3bf6c7e2283 71 led1 = !led1;
narshu 0:f3bf6c7e2283 72
narshu 0:f3bf6c7e2283 73 leftenc = motors.getEncoder1();
narshu 0:f3bf6c7e2283 74 rightenc = motors.getEncoder2();
narshu 0:f3bf6c7e2283 75
narshu 0:f3bf6c7e2283 76 dleft = motors.encoderToDistance(leftenc-lastleft)/1000.0f;
narshu 0:f3bf6c7e2283 77 dright = motors.encoderToDistance(rightenc-lastright)/1000.0f;
narshu 0:f3bf6c7e2283 78
narshu 0:f3bf6c7e2283 79 lastleft = leftenc;
narshu 0:f3bf6c7e2283 80 lastright = rightenc;
narshu 0:f3bf6c7e2283 81
narshu 0:f3bf6c7e2283 82
narshu 0:f3bf6c7e2283 83 //The below calculation are in body frame (where +x is forward)
narshu 0:f3bf6c7e2283 84 thetap = (dright - dleft)*PI / (float(robotCircumference)/1000.0f);
narshu 0:f3bf6c7e2283 85 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:f3bf6c7e2283 86 d = (dright + dleft)/2.0f;
narshu 0:f3bf6c7e2283 87 dxp = d*cos(thetap/2.0f);
narshu 0:f3bf6c7e2283 88 dyp = d*sin(thetap/2.0f);
narshu 0:f3bf6c7e2283 89
narshu 0:f3bf6c7e2283 90 } else { //calculate circle arc
narshu 0:f3bf6c7e2283 91 //float r = (right + left) / (4.0f * PI * thetap);
narshu 0:f3bf6c7e2283 92 r = (dright + dleft) / (2.0f*thetap);
narshu 0:f3bf6c7e2283 93 dxp = abs(r)*sin(thetap);
narshu 0:f3bf6c7e2283 94 dyp = r - r*cos(thetap);
narshu 0:f3bf6c7e2283 95 }
narshu 0:f3bf6c7e2283 96
narshu 0:f3bf6c7e2283 97 statelock.lock();
narshu 0:f3bf6c7e2283 98
narshu 0:f3bf6c7e2283 99 //rotating to cartesian frame and updating state
narshu 0:f3bf6c7e2283 100 X(0) += dxp * cos(X(2)) - dyp * sin(X(2));
narshu 0:f3bf6c7e2283 101 X(1) += dxp * sin(X(2)) + dyp * cos(X(2));
narshu 0:f3bf6c7e2283 102 X(2) = rectifyAng(X(2) + thetap);
narshu 0:f3bf6c7e2283 103
narshu 0:f3bf6c7e2283 104 //Linearising F around X
narshu 0:f3bf6c7e2283 105 F = 1, 0, (dxp * -sin(X(2)) - dyp * cos(X(2))),
narshu 0:f3bf6c7e2283 106 0, 1, (dxp * cos(X(2)) - dyp * sin(X(2))),
narshu 0:f3bf6c7e2283 107 0, 0, 1;
narshu 0:f3bf6c7e2283 108
narshu 0:f3bf6c7e2283 109
narshu 0:f3bf6c7e2283 110 P = F * P * trans(F) + Q;
narshu 0:f3bf6c7e2283 111
narshu 0:f3bf6c7e2283 112 statelock.unlock();
narshu 0:f3bf6c7e2283 113 Thread::wait(20);
narshu 0:f3bf6c7e2283 114
narshu 0:f3bf6c7e2283 115 //cout << "predict" << X << endl;
narshu 0:f3bf6c7e2283 116 //cout << P << endl;
narshu 0:f3bf6c7e2283 117 }
narshu 0:f3bf6c7e2283 118 }
narshu 0:f3bf6c7e2283 119
narshu 0:f3bf6c7e2283 120 //void Kalman::sonarloop() {
narshu 0:f3bf6c7e2283 121 // while (1) {
narshu 0:f3bf6c7e2283 122 // Thread::signal_wait(0x1);
narshu 0:f3bf6c7e2283 123 // sonararray.startRange();
narshu 0:f3bf6c7e2283 124 // }
narshu 0:f3bf6c7e2283 125 //}
narshu 0:f3bf6c7e2283 126
narshu 0:f3bf6c7e2283 127
narshu 0:f3bf6c7e2283 128 void Kalman::runupdate(measurement_t type, float value, float variance) {
narshu 0:f3bf6c7e2283 129 //printf("beacon %d dist %f\r\n", sonarid, dist);
narshu 0:f3bf6c7e2283 130 //led2 = !led2;
narshu 0:f3bf6c7e2283 131
narshu 0:f3bf6c7e2283 132 measurmentdata* measured = (measurmentdata*)measureMQ.alloc();
narshu 0:f3bf6c7e2283 133 if (measured) {
narshu 0:f3bf6c7e2283 134 measured->mtype = type;
narshu 0:f3bf6c7e2283 135 measured->value = value;
narshu 0:f3bf6c7e2283 136 measured->variance = variance;
narshu 0:f3bf6c7e2283 137
narshu 0:f3bf6c7e2283 138 osStatus putret = measureMQ.put(measured);
narshu 0:f3bf6c7e2283 139 if (putret)
narshu 0:f3bf6c7e2283 140 led4 = 1;
narshu 0:f3bf6c7e2283 141 // printf("putting in MQ error code %#x\r\n", putret);
narshu 0:f3bf6c7e2283 142 } else {
narshu 0:f3bf6c7e2283 143 led4 = 1;
narshu 0:f3bf6c7e2283 144 //printf("MQalloc returned NULL ptr\r\n");
narshu 0:f3bf6c7e2283 145 }
narshu 0:f3bf6c7e2283 146
narshu 0:f3bf6c7e2283 147 }
narshu 0:f3bf6c7e2283 148
narshu 0:f3bf6c7e2283 149 void Kalman::updateloop() {
narshu 0:f3bf6c7e2283 150 measurement_t type;
narshu 0:f3bf6c7e2283 151 float value,variance,rbx,rby,expecdist,Y;
narshu 0:f3bf6c7e2283 152 float dhdx,dhdy;
narshu 0:f3bf6c7e2283 153 bool aborton2stddev = false;
narshu 0:f3bf6c7e2283 154
narshu 0:f3bf6c7e2283 155 Matrix<float, 1, 3> H;
narshu 0:f3bf6c7e2283 156
narshu 0:f3bf6c7e2283 157 float S;
narshu 0:f3bf6c7e2283 158 Matrix<float, 3, 3> I3( identity< Matrix<float, 3, 3> >() );
narshu 0:f3bf6c7e2283 159
narshu 0:f3bf6c7e2283 160
narshu 0:f3bf6c7e2283 161 while (1) {
narshu 0:f3bf6c7e2283 162 led2 = !led2;
narshu 0:f3bf6c7e2283 163
narshu 0:f3bf6c7e2283 164 osEvent evt = measureMQ.get();
narshu 0:f3bf6c7e2283 165
narshu 0:f3bf6c7e2283 166 if (evt.status == osEventMail) {
narshu 0:f3bf6c7e2283 167
narshu 0:f3bf6c7e2283 168 measurmentdata &measured = *(measurmentdata*)evt.value.p;
narshu 0:f3bf6c7e2283 169 type = measured.mtype; //Note, may support more measurment types than sonar in the future!
narshu 0:f3bf6c7e2283 170 value = measured.value;
narshu 0:f3bf6c7e2283 171 variance = measured.variance;
narshu 0:f3bf6c7e2283 172
narshu 0:f3bf6c7e2283 173 // don't forget to free the memory
narshu 0:f3bf6c7e2283 174 measureMQ.free(&measured);
narshu 0:f3bf6c7e2283 175
narshu 0:f3bf6c7e2283 176 if (type <= maxmeasure) {
narshu 0:f3bf6c7e2283 177
narshu 0:f3bf6c7e2283 178 if (type <= SONAR3) {
narshu 0:f3bf6c7e2283 179
narshu 0:f3bf6c7e2283 180 float dist = value / 1000.0f; //converting to m from mm
narshu 0:f3bf6c7e2283 181 int sonarid = type;
narshu 0:f3bf6c7e2283 182 aborton2stddev = true;
narshu 0:f3bf6c7e2283 183
narshu 0:f3bf6c7e2283 184 statelock.lock();
narshu 0:f3bf6c7e2283 185 SonarMeasures[sonarid] = dist; //update the current sonar readings
narshu 0:f3bf6c7e2283 186
narshu 0:f3bf6c7e2283 187 rbx = X(0) - beaconpos[sonarid].x/1000.0f;
narshu 0:f3bf6c7e2283 188 rby = X(1) - beaconpos[sonarid].y/1000.0f;
narshu 0:f3bf6c7e2283 189
narshu 0:f3bf6c7e2283 190 expecdist = sqrt(rbx*rbx + rby*rby);
narshu 0:f3bf6c7e2283 191 Y = dist - expecdist;
narshu 0:f3bf6c7e2283 192
narshu 0:f3bf6c7e2283 193 dhdx = rbx / expecdist;
narshu 0:f3bf6c7e2283 194 dhdy = rby / expecdist;
narshu 0:f3bf6c7e2283 195
narshu 0:f3bf6c7e2283 196 H = dhdx, dhdy, 0;
narshu 0:f3bf6c7e2283 197
narshu 0:f3bf6c7e2283 198 } else if (type <= IR3) {
narshu 0:f3bf6c7e2283 199
narshu 0:f3bf6c7e2283 200 int IRidx = type-3;
narshu 0:f3bf6c7e2283 201
narshu 0:f3bf6c7e2283 202 statelock.lock();
narshu 0:f3bf6c7e2283 203 IRMeasures[IRidx] = value;
narshu 0:f3bf6c7e2283 204
narshu 0:f3bf6c7e2283 205 rbx = X(0) - beaconpos[IRidx].x/1000.0f;
narshu 0:f3bf6c7e2283 206 rby = X(1) - beaconpos[IRidx].y/1000.0f;
narshu 0:f3bf6c7e2283 207
narshu 0:f3bf6c7e2283 208 float expecang = atan2(-rbx, -rby) - X(2);
narshu 0:f3bf6c7e2283 209 Y = rectifyAng(value - expecang);
narshu 0:f3bf6c7e2283 210
narshu 0:f3bf6c7e2283 211 float dstsq = rbx*rbx + rby*rby;
narshu 0:f3bf6c7e2283 212 H = -rby/dstsq, rbx/dstsq, -1;
narshu 0:f3bf6c7e2283 213
narshu 0:f3bf6c7e2283 214 }
narshu 0:f3bf6c7e2283 215
narshu 0:f3bf6c7e2283 216 Matrix<float, 3, 1> PH (P * trans(H));
narshu 0:f3bf6c7e2283 217 S = (H * PH)(0,0) + variance;
narshu 0:f3bf6c7e2283 218
narshu 0:f3bf6c7e2283 219 if (aborton2stddev && Y*Y > 4 * S) {
narshu 0:f3bf6c7e2283 220 statelock.unlock();
narshu 0:f3bf6c7e2283 221 continue;
narshu 0:f3bf6c7e2283 222 }
narshu 0:f3bf6c7e2283 223
narshu 0:f3bf6c7e2283 224 Matrix<float, 3, 1> K (PH * (1/S));
narshu 0:f3bf6c7e2283 225
narshu 0:f3bf6c7e2283 226 //Updating state
narshu 0:f3bf6c7e2283 227 X += col(K, 0) * Y;
narshu 0:f3bf6c7e2283 228 X(2) = rectifyAng(X(2));
narshu 0:f3bf6c7e2283 229
narshu 0:f3bf6c7e2283 230 P = (I3 - K * H) * P;
narshu 0:f3bf6c7e2283 231
narshu 0:f3bf6c7e2283 232 statelock.unlock();
narshu 0:f3bf6c7e2283 233
narshu 0:f3bf6c7e2283 234 }
narshu 0:f3bf6c7e2283 235
narshu 0:f3bf6c7e2283 236 } else {
narshu 0:f3bf6c7e2283 237 led4 = 1;
narshu 0:f3bf6c7e2283 238 //printf("ERROR: in updateloop, code %#x", evt);
narshu 0:f3bf6c7e2283 239 }
narshu 0:f3bf6c7e2283 240
narshu 0:f3bf6c7e2283 241 }
narshu 0:f3bf6c7e2283 242
narshu 0:f3bf6c7e2283 243 }