-
Notifications
You must be signed in to change notification settings - Fork 7
/
kalman.js
129 lines (102 loc) · 2.54 KB
/
kalman.js
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
/**
* Kalman.js v0.0.2 26/01/2016
*
* Copyright (c) 2016, Robert Eisele (robert@xarg.org)
* Dual licensed under the MIT or GPL Version 2 licenses.
**/
(function(root) {
"use strict";
/**
* @constructor
* @returns {KF}
*/
function KF(X0, P0) {
this['x'] = X0;
this['P'] = P0;
}
KF.prototype = {
'x': null,
'P': null,
//
'update': function(ob) {
// x_k: Predicted State vector / Estimated signal
// X_k: State vector
var x = this['x']; // Get prev state
// p_k: Predicted Covariance Matrix
// P_k: Covariance Matrix
var P = this['P']; // Get prev cov
// A: Transition matrix that describes how the state transforms from a previous state to the current by multiplication
// B: Format Matrix
// C: Format Matrix
// H: Control Matrix
var A = ob['A'];
var B = ob['B'];
var C = ob['C'];
var H = ob['H'];
// U_k: Control Variable Matrix, like movement command
var u = ob['u'];
// Q_k: Process-Noise (Keeps state Cov-Matrix from becoming too small)
var Qk = ob['Q'];
// y_k: New Observation
// Y_k: Observed value
var y = ob['y'];
// K: Kalman Gain
// R: Sensor Noise Covariance
var R = ob['R'];
// Predict State
// x_k = A * X_{k-1} + B * U_k
var xhat = A.multiply(x).add(B.multiply(u));
// Predicted process Covariance Matrix
// P_k = A * P_{k-1} * A^t + Q_k
var Phat = A.multiply(P).multiply(A.transpose());
if (Qk) {
Phat = Phat.add(Qk);
}
// Kalman Gain, weight for measurement and model
// K = P_k * H^t * (H * p_k * H^t + R)^-1
var K = Phat.multiply(H.transpose());
var T = H.multiply(K).add(R);
K = K.multiply(T.inverse()); // TODO: OPTIMIZE!
// New Observation
// y_k = C * Y_k + Z_k
var yk = C.multiply(y);
// Update state
// X_k = x_k + K(y_k - H * x_k)
x = xhat.add(K.multiply(yk.subtract(H.multiply(xhat))));
// Update process Covariance Matrix
// P_k = (I - K * H) * p_k = OR = p_k - K * H * p_k
P = Phat.subtract(K.multiply(H).multiply(Phat));
// Set Current state
this['x'] = x;
this['P'] = P;
},
'getState': function() {
return this['x'];
}
};
function EKF() {
}
function UKF() {
}
if (typeof define === "function" && define["amd"]) {
define([], function() {
return {
KF: KF,
EKF: EKF,
UKF: UKF
};
});
} else if (typeof exports === "object") {
module["exports"] = {
KF: KF,
EKF: EKF,
UKF: UKF
};
} else {
root["Kalman"] = {
KF: KF,
EKF: EKF,
UKF: UKF
};
}
})(this);