677a20be26349695f4274a1f04b25d19066fbad0
[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(ysp)
190 {
191     this._ysp = ysp;
192     this._out = 0;
193
194     this._Kp = 0;
195     this._stage = PIDController.stages.WARMING;
196
197     this._eold = 0;
198     this._I = 0;
199 }
200
201 // This enum will be used to tell whether the system output (or the controller input)
202 // is moving towards the set-point or away from it.
203 PIDController.yPositions = {
204     BEFORE_SETPOINT: 0,
205     AFTER_SETPOINT: 1
206 }
207
208 // The Ziegler–Nichols method for is used tuning the PID controller. The workflow of
209 // the tuning is split into four stages. The first two stages determine the values
210 // of the PID controller gains. During these two stages we return the proportional
211 // term only. The third stage is used to determine the min-max values of the
212 // saturation actuator. In the last stage back-calculation and tracking are applied
213 // to avoid integrator windup. During the last two stages, we return a PID control
214 // value.
215 PIDController.stages = {
216     WARMING: 0,         // Increase the value of the Kp until the system output reaches ysp.
217     OVERSHOOT: 1,       // Measure the oscillation period and the overshoot value
218     UNDERSHOOT: 2,      // Return PID value and measure the undershoot value
219     SATURATE: 3         // Return PID value and apply back-calculation and tracking.
220 }
221
222 PIDController.prototype =
223 {
224     // Determines whether the current y is
225     //  before ysp => (below ysp if ysp > y0) || (above ysp if ysp < y0)
226     //  after ysp => (above ysp if ysp > y0) || (below ysp if ysp < y0)
227     _yPosition: function(y)
228     {
229         return (y < this._ysp) == (this._y0 < this._ysp)
230             ? PIDController.yPositions.BEFORE_SETPOINT
231             : PIDController.yPositions.AFTER_SETPOINT;
232     },
233
234     // Calculate the ultimate distance from y0 after time t. We want to move very
235     // slowly at the beginning to see how adding few items to the test can affect
236     // its output. The complexity of a single item might be big enough to keep the
237     // proportional gain very small but achieves the desired progress. But if y does
238     // not change significantly after adding few items, that means we need a much
239     // bigger gain. So we need to move over a cubic curve which increases very
240     // slowly with small t values but moves very fast with larger t values.
241     // The basic formula is: y = t^3
242     // Change the formula to reach y=1 after 1000 ms: y = (t/1000)^3
243     // Change the formula to reach y=(ysp - y0) after 1000 ms: y = (ysp - y0) * (t/1000)^3
244     _distanceUltimate: function(t)
245     {
246         return (this._ysp - this._y0) * Math.pow(t / 1000, 3);
247     },
248
249     // Calculates the distance of y relative to y0. It also ensures we do not return
250     // zero by returning a epsilon value in the same direction as ultimate distance.
251     _distance: function(y, du)
252     {
253         const epsilon = 0.0001;
254         var d  = y - this._y0;
255         return du < 0 ? Math.min(d, -epsilon) : Math.max(d, epsilon);
256     },
257
258     // Decides how much the proportional gain should be increased during the manual
259     // gain stage. We choose to use the ratio of the ultimate distance to the current
260     // distance as an indication of how much the system is responsive. We want
261     // to keep the increment under control so it does not cause the system instability
262     // So we choose to take the natural logarithm of this ratio.
263     _gainIncrement: function(t, y, e)
264     {
265         var du = this._distanceUltimate(t);
266         var d = this._distance(y, du);
267         return Math.log(du / d) * 0.1;
268     },
269
270     // Update the stage of the controller based on its current stage and the system output
271     _updateStage: function(y)
272     {
273         var yPosition = this._yPosition(y);
274
275         switch (this._stage) {
276         case PIDController.stages.WARMING:
277             if (yPosition == PIDController.yPositions.AFTER_SETPOINT)
278                 this._stage = PIDController.stages.OVERSHOOT;
279             break;
280
281         case PIDController.stages.OVERSHOOT:
282             if (yPosition == PIDController.yPositions.BEFORE_SETPOINT)
283                 this._stage = PIDController.stages.UNDERSHOOT;
284             break;
285
286         case PIDController.stages.UNDERSHOOT:
287             if (yPosition == PIDController.yPositions.AFTER_SETPOINT)
288                 this._stage = PIDController.stages.SATURATE;
289             break;
290         }
291     },
292
293     // Manual tuning is used before calculating the PID controller gains.
294     _tuneP: function(e)
295     {
296         // The output is the proportional term only.
297         return this._Kp * e;
298     },
299
300     // PID tuning function. Kp, Ti and Td were already calculated
301     _tunePID: function(h, y, e)
302     {
303         // Proportional term.
304         var P = this._Kp * e;
305
306         // Integral term is the area under the curve starting from the beginning
307         // till the current time.
308         this._I += (this._Kp / this._Ti) * ((e + this._eold) / 2) * h;
309
310         // Derivative term is the slope of the curve at the current time.
311         var D = (this._Kp * this._Td) * (e - this._eold) / h;
312
313         // The ouput is a PID function.
314        return P + this._I + D;
315     },
316
317     // Apply different strategies for the tuning based on the stage of the controller.
318     _tune: function(t, h, y, e)
319     {
320         switch (this._stage) {
321         case PIDController.stages.WARMING:
322             // This is the first stage of the Zieglerâ\80Nichols method. It increments
323             // the proportional gain till the system output passes the set-point value.
324             if (typeof this._y0 == "undefined") {
325                 // This is the first time a tuning value is required. We want the test
326                 // to add only one item. So we need to return -1 which forces us to
327                 // choose the initial value of Kp to be = -1 / e
328                 this._y0 = y;
329                 this._Kp = -1 / e;
330             } else {
331                 // Keep incrementing the Kp as long as we have not reached the
332                 // set-point yet
333                 this._Kp += this._gainIncrement(t, y, e);
334             }
335
336             return this._tuneP(e);
337
338         case PIDController.stages.OVERSHOOT:
339             // This is the second stage of the Zieglerâ\80Nichols method. It measures the
340             // oscillation period.
341             if (typeof this._t0 == "undefined") {
342                 // t is the time of the begining of the first overshot
343                 this._t0 = t;
344                 this._Kp /= 2;
345             }
346
347             return this._tuneP(e);
348
349         case PIDController.stages.UNDERSHOOT:
350             // This is the end of the Zieglerâ\80Nichols method. We need to calculate the
351             // integral and derivative periods.
352             if (typeof this._Ti == "undefined") {
353                 // t is the time of the end of the first overshot
354                 var Tu = t - this._t0;
355
356                 // Calculate the system parameters from Kp and Tu assuming
357                 // a "some overshoot" control type. See:
358                 // https://en.wikipedia.org/wiki/Ziegler%E2%80%93Nichols_method
359                 this._Ti = Tu / 2;
360                 this._Td = Tu / 3;
361                 this._Kp = 0.33 * this._Kp;
362
363                 // Calculate the tracking time.
364                 this._Tt = Math.sqrt(this._Ti * this._Td);
365             }
366
367             return this._tunePID(h, y, e);
368
369         case PIDController.stages.SATURATE:
370             return this._tunePID(h, y, e);
371         }
372
373         return 0;
374     },
375
376     // Ensures the system does not fluctuates.
377     _saturate: function(v, e)
378     {
379         var u = v;
380
381         switch (this._stage) {
382         case PIDController.stages.OVERSHOOT:
383         case PIDController.stages.UNDERSHOOT:
384             // Calculate the min-max values of the saturation actuator.
385             if (typeof this._min == "undefined")
386                 this._min = this._max = this._out;
387             else {
388                 this._min = Math.min(this._min, this._out);
389                 this._max = Math.max(this._max, this._out);
390             }
391             break;
392
393         case PIDController.stages.SATURATE:
394             const limitPercentage = 0.90;
395             var min = this._min > 0 ? Math.min(this._min, this._max * limitPercentage) : this._min;
396             var max = this._max < 0 ? Math.max(this._max, this._min * limitPercentage) : this._max;
397             var out = this._out + u;
398
399             // Clip the controller output to the min-max values
400             out = Math.max(Math.min(max, out), min);
401             u = out - this._out;
402
403             // Apply the back-calculation and tracking
404             if (u != v)
405                 u += (this._Kp * this._Tt / this._Ti) * e;
406             break;
407         }
408
409         this._out += u;
410         return u;
411     },
412
413     // Called from the benchmark to tune its test. It uses Ziegler–Nichols method
414     // to calculate the controller parameters. It then returns a PID tuning value.
415     tune: function(t, h, y)
416     {
417         this._updateStage(y);
418
419         // Current error.
420         var e = this._ysp - y;
421         var v = this._tune(t, h, y, e);
422
423         // Save e for the next call.
424         this._eold = e;
425
426         // Apply back-calculation and tracking to avoid integrator windup
427         return this._saturate(v, e);
428     }
429 }
430
431 function KalmanEstimator(initX)
432 {
433     // Initialize state transition matrix.
434     this._matA = Matrix3.identity();
435     this._matA[Matrix3.pos(0, 2)] = 1;
436
437     // Initialize measurement matrix.
438     this._vecH = Vector3.zeros();
439     this._vecH[0] = 1;
440
441     this._matQ = Matrix3.identity();
442     this._R = 1000;
443
444     // Initial state conditions.
445     this._vecX_est = Vector3.zeros();
446     this._vecX_est[0] = initX;
447     this._matP_est = Matrix3.zeros();
448 }
449
450 KalmanEstimator.prototype =
451 {
452     estimate: function(current)
453     {
454         // Project the state ahead
455         //  X_prd(k) = A * X_est(k-1)
456         var vecX_prd = Matrix3.multiplyVector3(this._matA, this._vecX_est);
457
458         // Project the error covariance ahead
459         //  P_prd(k) = A * P_est(k-1) * A' + Q
460         var matP_prd = Matrix3.add(Matrix3.multiplyMatrix3(Matrix3.multiplyMatrix3(this._matA, this._matP_est), Matrix3.transpose(this._matA)), this._matQ);
461
462         // Compute Kalman gain
463         //  B = H * P_prd(k)';
464         //  S = B * H' + R;
465         //  K(k) = (S \ B)';
466         var vecB = Vector3.multiplyMatrix3(this._vecH, Matrix3.transpose(matP_prd));
467         var S = Vector3.multiplyVector3(vecB, this._vecH) + this._R;
468         var vecGain = Vector3.scale(1/S, vecB);
469
470         // Update the estimate via z(k)
471         //  X_est(k) = x_prd + K(k) * (z(k) - H * X_prd(k));
472         this._vecX_est = Vector3.add(vecX_prd, Vector3.scale(current - Vector3.multiplyVector3(this._vecH, vecX_prd), vecGain));
473
474         // Update the error covariance
475         //  P_est(k) = P_prd(k) - K(k) * H * P_prd(k);
476         this._matP_est = Matrix3.subtract(matP_prd, Matrix3.scale(Vector3.multiplyVector3(vecGain, this._vecH), matP_prd));
477
478         // Compute the estimated measurement.
479         //  y = H * X_est(k);
480         return Vector3.multiplyVector3(this._vecH,  this._vecX_est);
481     }
482 }
483
484 function IdentityEstimator() {}
485 IdentityEstimator.prototype =
486 {
487     estimate: function(current)
488     {
489         return current;
490     }
491 };