blob: a5ac5bf04943f7256402e1827807d57386dfaef8 (
plain)
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
|
// Kalman Filter
//
// kalman_filter
//
// Kalman Filter
//
// Optimal state estimation via predict-update cycle
//
// Inputs: 1
// Outputs: 1
// Constants: 2
#include <math.h>
#include "ts.h"
typedef struct {
fp_t Q;
fp_t R;
} ts_kalman_filter_params_t;
ts_parameter_t const ts_kalman_filter_p = { .n_inputs = 1, .n_outputs = 1, .n_constants = 2, .params_size = sizeof(ts_kalman_filter_params_t) };
uint_t ts_kalman_filter_offset( ts_constant_t const *c )
{
( void )c;
return ( 0 );
}
int ts_kalman_filter(
fp_t * const *y, uint_t y_n,
fp_t const * const *x, uint_t x_n,
uint_t n, ts_constant_t const *c
)
{
TS_PROLOGUE( ts_kalman_filter_p );
if ( !c ) return -1;
TS_CHECK_INPUT(x, x_n, n);
fp_t Q = ((ts_kalman_filter_params_t const *)c)->Q;
fp_t R = ((ts_kalman_filter_params_t const *)c)->R;
if (Q < 0 || R < 0) return -1;
if (isnan(Q) || isnan(R)) return -1;
fp_t x_est = x[0][0];
/* NOTE: P=1.0 is a reasonable default for normalized data.
* For price data in the range of 100-3000, consider initializing
* P to the variance of the first few samples. */
fp_t P = 1.0;
y[0][0] = x_est;
for ( uint_t i = 1; i < n; ++i )
{
P = P + Q;
fp_t denom = P + R;
fp_t K = denom != 0 ? P / denom : 0;
x_est = x_est + K * (x[0][i] - x_est);
P = (1 - K) * P;
y[0][i] = x_est;
}
return 0;
}
|