c13833b41565bae837583fe38253bb9a2054ca44
[WebKit-https.git] / PerformanceTests / Animometer / tests / resources / math.js
1 var Matrix =
2 {
3     init: function(m, n, v)
4     {
5         return Array(m * n).fill(v);
6     },
7     
8     zeros: function(m, n)
9     {
10         return Matrix.init(m, n, 0);
11     },
12     
13     ones: function(m, n)
14     {
15         return Matrix.init(m, n, 1);
16     },
17     
18     identity: function(n)
19     {
20         var out = new Matrix.zeros(n, n);
21         for (var i = 0; i < n; ++i)
22             out[i * n + i] = 1;
23         return out;
24     },
25     
26     str: function(A, n, m)
27     {
28         var out = (m > 1 && n > 1 ? "Matrix[" + n + ", " + m : "Vector[" + m * n) + "] = [";
29         for (var i = 0; i < n * m; ++i) {
30             out += A[i];
31             if (i < n * m - 1)
32                 out += ", ";
33         }       
34         return out + "]";
35     },
36     
37     pos: function(m, i, j)
38     {
39         return m * i + j;
40     },
41     
42     add: function(A, B, n, m)
43     {
44         var out = Matrix.zeros(n, m);
45         for (var i = 0; i < n * m; ++i)
46             out[i] = A[i] + B[i];
47         return out;
48     },
49     
50     subtract: function(A, B, n, m)
51     {
52         var out = Matrix.zeros(n, m);
53         for (var i = 0; i < n * m; ++i)
54             out[i] = A[i] - B[i];
55         return out;
56     },
57     
58     scale: function(s, A, n, m)
59     {
60         var out = Matrix.zeros(n, m);
61         for (var i = 0; i < n * m; ++i)
62             out[i] = s * A[i];
63         return out;
64     },
65     
66     transpose: function(A, n, m)
67     {
68         var out = Matrix.zeros(m, n);
69         for (var i = 0; i < n; ++i) {
70             for (var j = 0; j < m; ++j)
71                 out[Matrix.pos(n, i, j)] = A[Matrix.pos(m, j, i)];
72         }       
73         return out;
74     },
75     
76     multiply: function(A, B, n, m, p)
77     {
78         var out = Matrix.zeros(n, p);
79         for (var i = 0; i < n; ++i) {
80             for (var j = 0; j < p; ++j) {
81                 for (var k = 0; k < m; ++k) {
82                     out[Matrix.pos(p, i, j)] += A[Matrix.pos(m, i, k)] * B[Matrix.pos(p, k, j)];
83                 }
84             }
85         }
86         return out;
87     }
88 }
89
90 var Vector3 =
91 {
92     zeros: function()
93     {
94         return Matrix.zeros(1, 3);
95     },
96     
97     ones: function()
98     {
99         return Matrix.ones(1, 3);
100     },
101     
102     str: function(v)
103     {
104         return Matrix.str(v, 1, 3);
105     },
106     
107     add: function(v, w)
108     {
109         return Matrix.add(v, w, 1, 3);
110     },
111     
112     subtract: function(v, w)
113     {
114         return Matrix.subtract(v, w, 1, 3);
115     },
116     
117     scale: function(s, v)
118     {
119         return Matrix.scale(s, v, 1, 3);
120     },
121     
122     multiplyMatrix3: function(v, A)
123     {
124         return Matrix.multiply(v, A, 1, 3, 3);
125     },
126     
127     multiplyVector3: function(v, w)
128     {
129         var out = 0;
130         for (var i = 0; i < 3; ++i)
131             out += v[i] * w[i];
132         return out;
133     }
134 }
135
136 var Matrix3 =
137 {
138     zeros: function()
139     {
140         return Matrix.zeros(3, 3);
141     },
142     
143     identity: function()
144     {
145         return Matrix.identity(3, 3);
146     },
147     
148     str: function(A)
149     {
150         return Matrix.str(A, 3, 3);
151     },
152     
153     pos: function(i, j)
154     {
155         return Matrix.pos(3, i, j);
156     },
157     
158     add: function(A, B)
159     {
160         return Matrix.add(A, B, 3, 3);
161     },
162     
163     subtract: function(A, B)
164     {
165         return Matrix.subtract(A, B, 3, 3);
166     },
167     
168     scale: function(s, A)
169     {
170         return Matrix.scale(s, A, 3, 3);
171     },
172     
173     transpose: function(A)
174     {
175         return Matrix.transpose(A, 3, 3);
176     },
177     
178     multiplyMatrix3: function(A, B)
179     {
180         return Matrix.multiply(A, B, 3, 3, 3);
181     },
182     
183     multiplyVector3: function(A, v)
184     {
185         return Matrix.multiply(A, v, 3, 3, 1);
186     }
187 }
188
189 function PIDController(K, ysp, ulow, uhigh)
190 {
191     this._ysp = ysp;
192
193     this._Td = 5;
194     this._Ti = 15;
195
196     this._Kp = K;
197     this._Kd = K / this._Td;
198     this._Ki = K / this._Ti;
199
200     this._eold = 0;
201     this._I = 0;
202 }
203
204 PIDController.prototype =
205 {
206     _sat: function(v, low, high)
207     {
208         return v < low ? low : (v > high ? high : v);
209     },
210     
211     tune: function(y, h)
212     {
213         // Current error.
214         var e = this._ysp - y;
215
216         // Proportional term.
217         var P = this._Kp * e;
218         
219         // Derivative term is the slope of the curve at the current time.
220         var D = this._Kd * (e - this._eold) / h;
221
222         // Integral term is the area under the curve starting from the begining till the current time.
223         this._I += this._Ki * ((e + this._eold) / 2) * h;
224
225         // pid controller value.
226         var v = P + D + this._I;
227         
228         // Avoid spikes by applying actuator saturation.
229         var u = this._sat(v, this._ulow, this._uhigh);        
230
231         this._eold = e;
232         return u;
233     }
234 }
235
236 function KalmanEstimator()
237 {
238     this._matA = Matrix3.identity();
239     
240     this._vecH = Vector3.zeros();
241     this._vecH[0] = 1;
242     
243     this._matQ = Matrix3.identity();
244     this._R = 1000;
245     
246     this._vecX_est = Vector3.ones();
247     this._matP_est = Matrix3.zeros();
248 }
249
250 KalmanEstimator.prototype =
251 {
252     estimate: function(timeDelta, current)
253     {
254         // Update the transition matrix.
255         this._matA[Matrix3.pos(0, 2)] = 1;
256
257         // Predicted state and covariance.
258         var vecX_prd = Matrix3.multiplyVector3(this._matA, this._vecX_est);
259         var matP_prd = Matrix3.add(Matrix3.multiplyMatrix3(Matrix3.multiplyMatrix3(this._matA, this._matP_est), Matrix3.transpose(this._matA)), this._matQ);
260
261         // Estimation.
262         var vecB = Vector3.multiplyMatrix3(this._vecH, Matrix3.transpose(matP_prd));
263         var S = Vector3.multiplyVector3(vecB, this._vecH) + this._R;
264
265         var vecGain = Vector3.scale(1/S, vecB);
266         
267         // Estimated state and covariance.
268         this._vecX_est = Vector3.add(vecX_prd, Vector3.scale(current - Vector3.multiplyVector3(this._vecH, vecX_prd), vecGain));
269         this._matP_est = Matrix3.subtract(matP_prd, Matrix3.scale(Vector3.multiplyVector3(vecGain, this._vecH), matP_prd));
270
271         // Compute the estimated measurement.
272         return Vector3.multiplyVector3(this._vecH,  this._vecX_est);
273     }
274 }