/
BKFilterNode.ts
173 lines (155 loc) · 4.86 KB
/
BKFilterNode.ts
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
import { PropertyFilterProcessingNode } from './PropertyFilterProcessingNode';
import { FilterProcessingOptions } from './FilterProcessingNode';
import { DataObject, DataFrame } from '../../../data';
import { Vector3 } from '../../../utils';
/**
* Basic Kalman Filter processing node
*
* @category Processing node
*/
export class BKFilterNode<InOut extends DataFrame> extends PropertyFilterProcessingNode<InOut> {
constructor(
propertySelector: (object: DataObject, frame?: InOut) => [any, PropertyKey],
options: KalmanFilterOptions,
) {
super(propertySelector, options);
}
public initFilter<T extends number | Vector3>(
object: DataObject,
value: T,
options: KalmanFilterOptions,
): Promise<any> {
return new Promise<any>((resolve) => {
Object.keys(options).forEach((key) => {
if (typeof (options as any)[key] === 'number' && ['R', 'Q', 'A', 'B', 'C'].includes(key)) {
(options as any)[key] = new Vector3((options as any)[key], 0, 0);
}
});
resolve({ x: undefined, cov: NaN, ...options });
});
}
public filter<T extends number | Vector3>(object: DataObject, value: T, filter: any): Promise<T> {
return new Promise<T>((resolve) => {
const kf = new KalmanFilter(filter.R, filter.Q, filter.A, filter.B, filter.C, filter.x, filter.cov);
const numeric = typeof value === 'number';
if (numeric) {
kf.filter(new Vector3(value as number, 0, 0));
} else {
kf.filter(value as Vector3);
}
// Save the node data
filter.x = kf.measurement;
filter.cov = kf.covariance;
if (numeric) {
resolve(kf.measurement.x);
} else {
resolve(kf.measurement);
}
});
}
}
export interface KalmanFilterOptions extends FilterProcessingOptions {
/** Process noise */
R: number | Vector3;
/** Measurement noise */
Q: number | Vector3;
/** State vector */
A: number | Vector3;
/** Control vector */
B: number | Vector3;
/** Measurement vector */
C: number | Vector3;
}
/**
* Basic Kalman Filter
*
* @author Wouter Bulten
* @see {@link http://github.com/wouterbulten/kalmanjs}
* @copyright Copyright 2015-2018 Wouter Bulten
* @license MIT
*/
class KalmanFilter<T extends Vector3> {
/** Process noise */
private _R: T;
/** Measurement noise */
private _Q: T;
/** State vector */
private _A: T;
/** Control vector */
private _B: T;
/** Measurement vector */
private _C: T;
/** Noise filtered estimated signal */
private _x: T;
/** Covariance */
private _cov: T;
constructor(R: T, Q: T, A: T, B: T, C: T, x: T, cov: T) {
this._R = R;
this._Q = Q;
this._A = A;
this._B = B;
this._C = C;
this._x = x;
this._cov = cov;
}
/**
* Filter a new value
*
* @param {number} z Measurement
* @param {number} u Control
* @returns {number} Filtered value
*/
public filter(z: Vector3, u?: Vector3): Vector3 {
if (this._x === undefined) {
const ct = new Vector3(1, 1, 1).divide(this._C);
this._x = ct.clone().multiply(z) as T;
this._cov = ct.clone().multiply(this._Q).multiply(ct) as T;
} else {
// Compute prediction
const predX = this.predict(u);
const predCov = this.uncertainty();
// Kalman gain
const K = predCov
.clone()
.multiply(this._C)
.multiply(
new Vector3(1, 1, 1).divide(this._C.clone().multiply(predCov).multiply(this._C).add(this._Q)),
);
// Correction
this._x = predX.clone().add(K.clone().multiply(z.clone().sub(this._C.clone().multiply(predX)))) as T;
this._cov = predCov.clone().sub(K.clone().multiply(this._C).multiply(predCov));
}
return this._x;
}
/**
* Predict next value
*
* @param {number} [u] Control
* @returns {number} Predicted value
*/
public predict(u?: Vector3): Vector3 {
return this._A
.clone()
.multiply(this._x)
.add(u === undefined ? new Vector3() : this._B.clone().multiply(u));
}
/**
* Return uncertainty of filter
*
* @returns {number} Uncertainty
*/
public uncertainty(): T {
return this._A.clone().multiply(this._cov).multiply(this._A).add(this._R);
}
/**
* Return the last filtered measurement
*
* @returns {number} Last measurement
*/
public get measurement(): T {
return this._x;
}
public get covariance(): T {
return this._cov;
}
}