1 /*
2 * FreeSWITCH Modular Media Switching Software Library / Soft-Switch Application
3 * Copyright (C) 2005-2015, Anthony Minessale II <anthm@freeswitch.org>
4 *
5 * Version: MPL 1.1
6 *
7 * The contents of this file are subject to the Mozilla Public License Version
8 * 1.1 (the "License"); you may not use this file except in compliance with
9 * the License. You may obtain a copy of the License at
10 * http://www.mozilla.org/MPL/
11 *
12 * Software distributed under the License is distributed on an "AS IS" basis,
13 * WITHOUT WARRANTY OF ANY KIND, either express or implied. See the License
14 * for the specific language governing rights and limitations under the
15 * License.
16 *
17 * The Original Code is FreeSWITCH Modular Media Switching Software Library / Soft-Switch Application
18 *
19 * The Initial Developer of the Original Code is
20 * Anthony Minessale II <anthm@freeswitch.org>
21 * Portions created by the Initial Developer are Copyright (C)
22 * the Initial Developer. All Rights Reserved.
23 *
24 * Contributor(s):
25 *
26 * Dragos Oancea <droancea@yahoo.com>
27 *
28 * switch_estimators.c -- Estimators and Detectors (try to read into the future: packet loss, jitter, RTT, etc)
29 *
30 */
31
32 #include <switch.h>
33
34 #ifndef _MSC_VER
35 #include <switch_private.h>
36 #endif
37 #undef PACKAGE_NAME
38 #undef PACKAGE_STRING
39 #undef PACKAGE_TARNAME
40 #undef PACKAGE_VERSION
41 #undef PACKAGE_BUGREPORT
42 #undef VERSION
43 #undef PACKAGE
44 #undef inline
45 #include <switch_types.h>
46
47 #define KALMAN_SYSTEM_MODELS 3 /*loss, jitter, rtt*/
48 #define EST_LOSS 0
49 #define EST_JITTER 1
50 #define EST_RTT 2
51
52 /* This function initializes the Kalman System Model
53 *
54 * xk+1 = A*xk + wk
55 * zk = H*xk + vk
56 * xk = state variable (must exist in physical world - measurable )
57 * zk = measurment
58 * wk,vk - white noise
59 * A = state trasition matrix , (n x n ) matrix
60 * H = state-to-measurment matrix , ( n x n ) matrix
61 * Noise covariance:
62 * Q: Covariance matrix of wk, ( n x n ) diagonal matrix
63 * R: Covariance matrix of vk , ( m x m ) diagonal matrix
64 * R: if you want to be affected less by the measurement and get the estimate with less variation, increase R
65 * Q: if you want to be affected more by the measurement and get the estimate with more variation, decrease Q
66 *
67 * (Phil Kim book)
68 *
69 */
switch_kalman_init(kalman_estimator_t * est,float Q,float R)70 SWITCH_DECLARE(void) switch_kalman_init(kalman_estimator_t *est, float Q, float R)
71 {
72 est -> val_estimate_last = 0 ;
73 est -> P_last = 0;
74 est -> Q = Q; /*accuracy of system model */ /* SYSTEM MODEL: TO BE DEDUCTED */
75 est -> R = R; /*accuracy of measurement*/ /* SYSTEM MODEL: TO BE DEDUCTED */
76 est -> K = 0;
77 est -> val_estimate = 0 ;
78 est -> val_measured = 0 ; // [0-100 %] or [0-5000] or [0-2sec]
79 }
80
81 /*
82 CUSUM Kalman functions to detect sudden change over a predefined thereshold.
83
84 y(t) = sampled RTT
85 x(t)= desired RTT
86
87 Model:
88 x(t+1) = x(t) + delta(t)*v(t)
89 y(t) = x(t) + e(t)
90
91 Noisy characteristic of RTT captured by measurment noise e(t) with variance Re.
92 The step changes in the desired RTT x(t) is modeled as the process noise v(t)
93 with variance Rv and the discrete variable delta(t) .
94 If a change occurs at time t, then delta(t) = 1 otherwise delta(t) = 0.
95
96 avg(x(t)) = avg(x(t-1)) + K(t)(y(t) - avg(x(t-1)))
97 K(t) = P(t-1)/(P(t-1) + Re))
98 P(t) = (1-K(t))P(t-1) + delta(t-1)* Rv
99 e(t) = y(t) - avg(x(t))
100 g(t) = max(g(t-1) + e(t) - epsilon,0)
101 if g(t) > 0 then
102 delta(t) = 1 // alarm
103 g(t) = 0
104 else
105 delta(t) = 0
106 endif
107
108 constants:
109
110 epsilon = 0.005
111 h = 0.05
112 */
switch_kalman_cusum_init(cusum_kalman_detector_t * detect_change,float epsilon,float h)113 SWITCH_DECLARE(switch_bool_t) switch_kalman_cusum_init(cusum_kalman_detector_t *detect_change, float epsilon,float h)
114 {
115 cusum_kalman_detector_t *detector_change = detect_change;
116
117
118 if (epsilon < 0 || h < 0) {
119 return FALSE;
120 }
121
122 detector_change -> val_estimate_last = 0;
123 detector_change -> val_desired_last = 0;
124 detector_change -> P_last = 0;
125 detector_change -> K_last = 0;
126 detector_change -> delta = 0;
127 detector_change -> measurement_noise_e = 0;
128 detector_change -> variance_Re = 0;
129 detector_change -> measurement_noise_v = 0;
130 detector_change -> variance_Rv = 0;
131 detector_change -> g_last = 0;
132 /*per system model*/
133 detector_change -> epsilon = epsilon;
134 detector_change -> h = h;
135 /*variance*/
136 detector_change -> last_average = 0;
137 detector_change -> last_q = 0;
138 detector_change -> N = 0;
139 return TRUE;
140 }
141
switch_kalman_cusum_detect_change(cusum_kalman_detector_t * detector,float measurement,float rtt_avg)142 SWITCH_DECLARE (switch_bool_t) switch_kalman_cusum_detect_change(cusum_kalman_detector_t * detector, float measurement, float rtt_avg)
143 {
144 float K=0;
145 float P=0;
146 float g=0;
147 float desired_val;
148 float current_average;
149 float current_q;
150 float sample_variance_Re = 0;
151
152 /*variance*/
153
154 detector->N++;
155 current_average = detector->last_average + (measurement - detector->last_average)/detector->N ;
156 if (rtt_avg > current_average) {
157 current_average = rtt_avg;
158 }
159 current_q = detector-> last_q + (measurement - detector->last_average) * (measurement - current_average);
160 if (detector->N != 0)
161 sample_variance_Re = sqrt(current_q/detector->N);
162
163 detector->variance_Re = sample_variance_Re;
164 detector->variance_Rv = sample_variance_Re;
165
166 if (sample_variance_Re != 0) {
167 K = detector->P_last / (detector->P_last + detector->variance_Re);
168 desired_val = detector->val_desired_last + K * (measurement - detector->variance_Re);
169 P = (1 - K) * detector->P_last + detector->delta * detector->variance_Rv;
170 detector->measurement_noise_e = measurement - desired_val;
171 g = detector->g_last + detector->measurement_noise_e - detector->epsilon;
172 if (g > detector->h) {
173 detector->delta = 1;
174 g = 0;
175 } else {
176 detector->delta = 0;
177 }
178
179 /* update last vals for calculating variance */
180 detector->last_average = current_average;
181 /* update lasts (cusum)*/
182 detector -> g_last = g;
183 detector -> P_last = P;
184 detector -> val_desired_last = desired_val;
185 }
186 if (detector->delta == 1) {
187 return TRUE;
188 }
189 return FALSE;
190 }
191
192 /* Kalman filter abstract ( measure and estimate 1 single value per system model )
193 * Given the measurment and the system model together with the current state ,
194 * the function puts an estimate in the estimator struct */
switch_kalman_estimate(kalman_estimator_t * est,float measurement,int system_model)195 SWITCH_DECLARE(switch_bool_t) switch_kalman_estimate(kalman_estimator_t * est, float measurement, int system_model)
196 {
197 /*system model can be about: loss, jitter, rtt*/
198 float val_estimate;
199 float val_temp_est = est->val_estimate_last;
200 float P_temp = est->P_last + est->Q;
201
202 if (system_model >= KALMAN_SYSTEM_MODELS) {
203 return SWITCH_FALSE ;
204 }
205
206 /*sanitize input a little bit, just in case */
207 if (system_model == EST_LOSS ) {
208 if ((measurement > 100) || (measurement < 0)) {
209 return SWITCH_FALSE ;
210 }
211 }
212
213 if (system_model == EST_JITTER) {
214 if ((measurement > 10000) || (measurement < 0)) {
215 return SWITCH_FALSE;
216 }
217 }
218
219 if (system_model == EST_RTT) {
220 if ((measurement > 2 ) || (measurement < 0)) {
221 return SWITCH_FALSE;
222 }
223 }
224
225 /* calculate the Kalman gain */
226 est->K = P_temp * (1.0/(P_temp + est->R));
227 /* real life measurement */
228 est->val_measured = measurement ;
229 val_estimate = val_temp_est + est->K * (est->val_measured - val_temp_est);
230 est->P = (1 - est->K) * P_temp;
231 /*update lasts*/
232 est->P_last = est->P;
233 /* save the estimated value (future) */
234 est->val_estimate_last = val_estimate;
235 return SWITCH_TRUE;
236 }
237
switch_kalman_is_slow_link(kalman_estimator_t * est_loss,kalman_estimator_t * est_rtt)238 SWITCH_DECLARE(switch_bool_t) switch_kalman_is_slow_link(kalman_estimator_t * est_loss, kalman_estimator_t * est_rtt)
239 {
240 float thresh_packet_loss = 5; /* % */
241 float thresh_rtt = 0.8 ; /*seconds*/
242
243 if ((est_loss->val_estimate_last > thresh_packet_loss) &&
244 (est_rtt->val_estimate_last > thresh_rtt )) {
245 return SWITCH_TRUE;
246 }
247
248 return SWITCH_FALSE;
249 }
250
251 /* For Emacs:
252 * Local Variables:
253 * mode:c
254 * indent-tabs-mode:t
255 * tab-width:4
256 * c-basic-offset:4
257 * End:
258 * For VIM:
259 * vim:set softtabstop=4 shiftwidth=4 tabstop=4 noet:
260 */
261
262