1 /*
2 * The oRTP library is an RTP (Realtime Transport Protocol - rfc3550) implementation with additional features.
3 * Copyright (C) 2017 Belledonne Communications SARL
4 *
5 * This program is free software; you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation; either version 2 of the License, or
8 * (at your option) any later version.
9 *
10 * This program is distributed in the hope that it will be useful,
11 * but WITHOUT ANY WARRANTY; without even the implied warranty of
12 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 * GNU General Public License for more details.
14 *
15 * You should have received a copy of the GNU General Public License
16 * along with this program; if not, write to the Free Software
17 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
18 */
19
20 #include "ortp/utils.h"
21
22
ortp_kalman_rls_init(OrtpKalmanRLS * rls,double m0,double b0)23 void ortp_kalman_rls_init(OrtpKalmanRLS *rls, double m0, double b0) {
24 memset(rls, 0, sizeof(OrtpKalmanRLS));
25 rls->lambda = 1.;
26 rls->P[0][0] = 1e-10;
27 rls->P[1][1] = 1e-1;
28 rls->m = m0;
29 rls->b = b0;
30 }
31
ortp_kalman_rls_record(OrtpKalmanRLS * rls,double xmes,double ymes)32 void ortp_kalman_rls_record(OrtpKalmanRLS *rls, double xmes, double ymes) {
33 // P = a b
34 // c d
35 double a = rls->P[0][0];
36 double b = rls->P[0][1];
37 double c = rls->P[1][0];
38 double d = rls->P[1][1];
39 double e = xmes;
40 double f = 1;
41
42 double estim = rls->m * e + rls->b * f;
43 double deno = rls->lambda + (e * a + f * b) * e + (e * c + f * d) * f;
44
45 /** This is delta between the measure and our prediction based on previous model values:
46 the more accurate the system, the smaller this value.
47 **/
48 double diff = ymes - estim;
49
50 rls->m = rls->m + diff * (a*e+b*f);
51 rls->b = rls->b + diff * (c*e+d*f);
52
53 rls->P[0][0] = (a - (e*a+f*b)*(e*a+f*c) / deno) * 1.f / rls->lambda;
54 rls->P[1][0] = (b - (e*a+f*b)*(e*b+f*d) / deno) * 1.f / rls->lambda;
55 rls->P[0][1] = (c - (e*c+f*d)*(e*a+f*c) / deno) * 1.f / rls->lambda;
56 rls->P[1][1] = (d - (e*c+f*d)*(e*b+f*d) / deno) * 1.f / rls->lambda;
57 }
58
59
60