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