Skip to content

Commit 97c14f5

Browse files
piercuspiercus
authored
Merge pull request #61 from piercus/bouncing2
Bouncing2
2 parents 97980f6 + e342527 commit 97c14f5

23 files changed

+19217
-7669
lines changed

‎README.md‎

Lines changed: 12 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -14,8 +14,8 @@ This library implements following features:
1414

1515
## Demos/Examples
1616

17-
* [Sinusoidale Extended Kalman-Filter](https://observablehq.com/d/a033acc0859cc0de)
1817
* [Browser interactive constant acceleration on bikes](http://piercus.github.io/kalman-filter)
18+
* [Sinusoidale Extended Kalman-Filter](https://observablehq.com/d/a033acc0859cc0de)
1919
* [Code pen GPS Data smoothing with constant speed](https://codepen.io/piercus/pen/wvoqPww)
2020
* [Partial Observation](https://github.com/piercus/kalman-filter/issues/34)
2121
* [Smooth 3x3 rotation matrix](https://github.com/piercus/kalman-filter/issues/37)
@@ -137,6 +137,7 @@ For advanced usage, here is the correspondance table with the matrix name of the
137137
| $H_k$, the observation model | `observation.stateProjection` |
138138
| $Q_k$, the covariance of the process noise | `dynamic.covariance` |
139139
| $R_k$, the covariance of the observation noise | `observation.covariance` |
140+
| $B_k u_k$, the control-input model multiplied by the control vector | `dynamic.constant` |
140141
|$\mathbf{P}_{0\mid 0}$| `dynamic.init.covariance` |
141142
|$\mathbf{x}_{0\mid 0}$| `dynamic.init.mean` |
142143

@@ -151,6 +152,7 @@ Available default models as :
151152
* constant-acceleration
152153

153154
This will automatically configure the `dynamic.transition` matrix.
155+
154156
##### constant-position
155157

156158
```math
@@ -371,6 +373,7 @@ In order to use the Kalman-Filter with a dynamic or observation model which is n
371373
* `observation.covariance`
372374
* `dynamic.transition`
373375
* `dynamic.covariance`
376+
* `dynamic.constant`
374377

375378
In this situation this `function` will return the value of the matrix at each step of the kalman-filter.
376379

@@ -455,7 +458,14 @@ You will need to put your non-linear functions in the following parameters
455458
* `observation.fn`
456459
* `dynamic.fn`
457460

458-
See an example in `test/issues/56.js`
461+
See an example in [Sinusoidale Extended Kalman-Filter](https://observablehq.com/d/a033acc0859cc0de)
462+
463+
### Using Control model
464+
465+
If you want to add a constant parameter in the dynamic model (also called `control input`), you can use `dynamic.constant` function
466+
467+
See an example code in `demo/bouncing-ball` or the result in [Bouncing Ball example](https://observablehq.com/d/a033acc0859cc0de)
468+
459469

460470
## Use your kalman filter
461471

‎demo/src/kf-options.js‎ renamed to ‎demo/bike/kf-options.js‎

File renamed without changes.

‎demo/src/main.js‎ renamed to ‎demo/bike/main.js‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -2,8 +2,8 @@ const {KalmanFilter} = kalmanFilter;// eslint-disable-line no-undef
22

33
const noisyObservations = require('./observations.json').observations;
44
const kfOptions = require('./kf-options.js');
5-
const createElement = require('./views/create-element');
6-
const createGroupBoxes = require('./views/create-group-boxes');
5+
const createElement = require('../shared/views/create-element');
6+
const createGroupBoxes = require('../shared/views/create-group-boxes');
77

88
const kf = new KalmanFilter(kfOptions);
99
let predicted = kf.predict();

‎demo/src/observation-covariance.json‎ renamed to ‎demo/bike/observation-covariance.json‎

File renamed without changes.

‎demo/src/observations.json‎ renamed to ‎demo/bike/observations.json‎

File renamed without changes.

‎demo/bouncing-ball/kf-options.js‎

Lines changed: 72 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,72 @@
1+
2+
const posVar = 10;
3+
const timeStep = 1;
4+
const huge = 1000;
5+
const floor = 438;
6+
7+
module.exports = {
8+
observation: {
9+
dimension: 2,
10+
stateProjection: [
11+
[1, 0, 0, 0, 0],
12+
[0, 1, 0, 0, 0]
13+
],
14+
// Covariance generated thanks to getCovariance
15+
covariance: [posVar / 5, posVar / 5]
16+
// Covariance: [posVar, posVar, posVar, posVar],
17+
18+
},
19+
20+
dynamic: {
21+
init: {
22+
mean: [[943], [385], [0], [0], [0]],
23+
24+
covariance: [
25+
[huge, 0, 0, 0, 0],
26+
[0, huge, 0, 0, 0],
27+
[0, 0, huge, 0, 0],
28+
[0, 0, 0, huge, 0],
29+
[0, 0, 0, 0, huge]
30+
]
31+
},
32+
33+
constant({previousCorrected}) {
34+
const normalY = previousCorrected.mean[1][0] + (previousCorrected.mean[3][0] * timeStep);
35+
36+
let controlY = 0;
37+
38+
if (normalY > floor) {
39+
controlY = 2 * (floor - normalY);
40+
}
41+
42+
return [[0], [controlY], [0], [0], [0]];
43+
},
44+
45+
transition({previousCorrected}) {
46+
const normalY = previousCorrected.mean[1][0] + (previousCorrected.mean[3][0] * timeStep);
47+
let vY = 1;
48+
49+
if (normalY > floor) {
50+
vY = -1;
51+
}
52+
53+
return [
54+
[1, 0, timeStep, 0, 0],
55+
[0, 1, 0, timeStep, 0],
56+
[0, 0, 1, 0, 0],
57+
[0, 0, 0, vY, timeStep],
58+
[0, 0, 0, 0, 1]
59+
];
60+
},
61+
62+
dimension: 5,
63+
64+
covariance: [
65+
posVar,
66+
posVar,
67+
posVar * timeStep * timeStep,
68+
posVar * timeStep * timeStep,
69+
posVar * (timeStep ** 4)
70+
]
71+
}
72+
};

‎demo/bouncing-ball/main.js‎

Lines changed: 64 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,64 @@
1+
const {KalmanFilter} = kalmanFilter;// eslint-disable-line no-undef
2+
3+
const noisyObservations = require('./observations.json').observations;
4+
const kfOptions = require('./kf-options.js');
5+
const createElement = require('../shared/views/create-element');
6+
const createGroupPoint = require('../shared/views/create-group-point');
7+
8+
const kf = new KalmanFilter(kfOptions);
9+
let predicted = kf.predict();
10+
11+
const img = document.querySelector('#bouncing-ball');// eslint-disable-line no-undef
12+
13+
// Create all the elements of the prediction or correction phase
14+
const delay = 200;
15+
16+
let promise = Promise.resolve();
17+
let previousCorrected = null;
18+
19+
const delayPromise = delay => new Promise(resolve => {
20+
setTimeout(resolve, delay);
21+
});
22+
23+
module.exports = {
24+
run() {
25+
noisyObservations.forEach((box, index) => {
26+
promise = promise
27+
.then(() => {
28+
predicted = kf.predict({previousCorrected});
29+
const {mean, covariance} = predicted;
30+
31+
createGroupPoint({mean, covariance, parent: img, className: 'predicted', color: 'blue'});
32+
33+
return delayPromise(delay);
34+
})
35+
.then((b => {
36+
console.log({b});
37+
const w = 10;
38+
const h = 10;
39+
createElement({
40+
className: 'observation',
41+
bbox: [
42+
b[0],
43+
b[1],
44+
w,
45+
h
46+
],
47+
parent: img,
48+
color: 'white',
49+
lineStyle: 'solid'
50+
});
51+
52+
return delayPromise(delay);
53+
}).bind(null, box, index))
54+
.then((b => {
55+
previousCorrected = kf.correct({predicted, observation: b});
56+
const {mean, covariance} = previousCorrected;
57+
58+
createGroupPoint({mean, covariance, parent: img, className: 'corrected', color: 'red'});
59+
60+
return delayPromise(delay);
61+
}).bind(null, box, index));
62+
});
63+
}
64+
};

‎demo/bouncing-ball/observations.json‎

Lines changed: 175 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,175 @@
1+
{
2+
"ground": {
3+
y: 442
4+
},
5+
"observations": [
6+
[
7+
15,
8+
40
9+
],
10+
[
11+
39,
12+
70
13+
],
14+
[
15+
62,
16+
106
17+
],
18+
[
19+
83,
20+
148
21+
],
22+
[
23+
105,
24+
197
25+
],
26+
[
27+
126,
28+
253
29+
],
30+
[
31+
145,
32+
315
33+
],
34+
[
35+
165,
36+
383
37+
],
38+
[
39+
183,
40+
423
41+
],
42+
[
43+
202,
44+
364
45+
],
46+
[
47+
219,
48+
315
49+
],
50+
[
51+
236,
52+
274
53+
],
54+
[
55+
253,
56+
241
57+
],
58+
[
59+
270,
60+
216
61+
],
62+
[
63+
286,
64+
198
65+
],
66+
[
67+
302,
68+
187
69+
],
70+
[
71+
318,
72+
184
73+
],
74+
[
75+
334,
76+
187
77+
],
78+
[
79+
350,
80+
197
81+
],
82+
[
83+
364,
84+
214
85+
],
86+
[
87+
378,
88+
236
89+
],
90+
[
91+
393,
92+
265
93+
],
94+
[
95+
406,
96+
301
97+
],
98+
[
99+
419,
100+
343
101+
],
102+
[
103+
432,
104+
391
105+
],
106+
[
107+
443,
108+
434
109+
],
110+
[
111+
456,
112+
390
113+
],
114+
[
115+
469,
116+
354
117+
],
118+
[
119+
481,
120+
326
121+
],
122+
[
123+
494,
124+
305
125+
],
126+
[
127+
506,
128+
292
129+
],
130+
[
131+
519,
132+
285
133+
],
134+
[
135+
531,
136+
285
137+
],
138+
[
139+
544,
140+
291
141+
],
142+
[
143+
556,
144+
305
145+
],
146+
[
147+
568,
148+
323
149+
],
150+
[
151+
580,
152+
350
153+
],
154+
[
155+
591,
156+
380
157+
],
158+
[
159+
602,
160+
419
161+
],
162+
[
163+
613,
164+
420
165+
],
166+
[
167+
624,
168+
390
169+
],
170+
[
171+
613,
172+
420
173+
]
174+
]
175+
}

‎demo/src/views/create-arrow.js‎ renamed to ‎demo/shared/views/create-arrow.js‎

File renamed without changes.

‎demo/src/views/create-custom-dashed-line.js‎ renamed to ‎demo/shared/views/create-custom-dashed-line.js‎

File renamed without changes.

0 commit comments

Comments
 (0)