| 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
|
} |
Shuto Naruse