Well, they are:
/*yafl_config.h*/
#ifndef YAFL_CONFIG_H
#define YAFL_CONFIG_H
#include <math.h>
#include <stdint.h>
#ifdef DEBUG
/*
In this example we will use standard output.
You can actually use any printf implementation you want.
*/
# include <stdio.h>
# define YAFL_LOG(...) fprintf(stderr, __VA_ARGS__)
/*
Using branch speculation may save some clocks...
*/
# ifdef __GNUC__
# define YAFL_UNLIKELY(x) __builtin_expect((x), 0)
# else /*__GNUC__*/
# define YAFL_UNLIKELY(x) (x)
# endif/*__GNUC__*/
#else /*DEBUG*/
/*
Here we have "Never" actually, but you can use some of above definitions if you want.
*/
# define YAFL_UNLIKELY(x) (0)
#endif/*DEBUG*/
#define YAFL_EPS (1.0e-7)
#define YAFL_SQRT sqrtf
#define YAFL_ABS fabs
typedef float yaflFloat;
typedef int32_t yaflInt;
/* WARNING!!!
Fast UKF SSR updates may give dramatically incorrect results in case of adaptive Bierman filter
*/
//#define YAFL_USE_FAST_UKF
#endif // YAFL_CONFIG_H
Now you should:
#include <yafl.h>
/*OK! We can do some filtering...*/
#define NX 4 /* State vector dimension */
#define NZ 2 /* Observation vectio dimention */
/* This is our state transition function */
yaflStatusEn fx(yaflKalmanBaseSt * self, yaflFloat * x, yaflFloat * xz)
{
(void)xz;
YAFL_CHECK(self, YAFL_ST_INV_ARG_1);
YAFL_CHECK(4 == self->Nx, YAFL_ST_INV_ARG_1);
YAFL_CHECK(x, YAFL_ST_INV_ARG_2);
/*We have a linear uniform motion here:*/
x[0] += 0.1 * x[2];
x[1] += 0.1 * x[3];
return YAFL_ST_OK;
}
/* This is state transition Jacobian function */
yaflStatusEn jfx(yaflKalmanBaseSt * self, yaflFloat * w, yaflFloat * x)
{
yaflInt i;
yaflInt nx;
yaflInt nx2;
(void)x;
YAFL_CHECK(self, YAFL_ST_INV_ARG_1);
YAFL_CHECK(4 == self->Nx, YAFL_ST_INV_ARG_1);
YAFL_CHECK(w, YAFL_ST_INV_ARG_2);
nx = self->Nx;
nx2 = nx * 2;
/* W has: nx rows and 2*nx columns */
/* We must put our Jacobian to left side of W */
for (i = 0; i < nx; i++)
{
yaflInt j;
yaflInt nci;
nci = nx2 * i;
for (j = 0; j < nx; j++)
{
w[nci + j] = (i != j) ? 0.0 : 1.0;
}
}
w[nx2*0 + 2] = 0.1;
w[nx2*1 + 3] = 0.1;
/*
Now W is
(JFX|*)
where JFX is:
| 1 0 .1 0 |
| 0 1 0 .1 |
| 0 0 1 0 |
| 0 0 0 1 |
*/
return YAFL_ST_OK;
}
/*This is our measurement function*/
yaflStatusEn hx(yaflKalmanBaseSt * self, yaflFloat * y, yaflFloat * x)
{
YAFL_CHECK(self, YAFL_ST_INV_ARG_1);
YAFL_CHECK(2 == self->Nz, YAFL_ST_INV_ARG_1);
YAFL_CHECK(y, YAFL_ST_INV_ARG_2);
YAFL_CHECK(x, YAFL_ST_INV_ARG_3);
/*We see only coordinates, not the velocities*/
y[0] = x[0];
y[1] = x[1];
return YAFL_ST_OK;
}
/* This is hx Jacobian function */
yaflStatusEn jhx(yaflKalmanBaseSt * self, yaflFloat * h, yaflFloat * x)
{
yaflInt i;
yaflInt nx;
yaflInt nz;
YAFL_CHECK(self, YAFL_ST_INV_ARG_1);
YAFL_CHECK(4 == self->Nx, YAFL_ST_INV_ARG_1);
YAFL_CHECK(2 == self->Nz, YAFL_ST_INV_ARG_1);
YAFL_CHECK(h, YAFL_ST_INV_ARG_2);
YAFL_CHECK(x, YAFL_ST_INV_ARG_3);
nx = self->Nx;
nz = self->Nz;
/* Here we will use a full H matrix which has nz rows and nx columns */
for (i = 0; i < nz; i++)
{
yaflInt j;
yaflInt nci;
nci = nx * i;
for (j = 0; j < nx; j++)
{
h[nci + j] = 0.0;
}
}
h[nx*0 + 0] = 1.0;
h[nx*1 + 1] = 1.0;
/*
Now h is:
| 1 0 0 0 |
| 0 1 0 0 |
*/
return YAFL_ST_OK;
}
/*---------------------------------------------------------------------------*/
/*This is our filter memory structure*/
typedef struct
{
YAFL_EKF_BASE_MEMORY_MIXIN(NX, NZ); /*This mixin is actually used to declare EKF data storage*/
yaflFloat dummy[30]; /*We may need some additional memory...*/
} kfMemorySt;
/*Now we will initiate our memory*/
/*Some constants*/
#define DP (0.1)
#define DX (1.0e-6)
#define DZ (400)
kfMemorySt kf_memory =
{
/*Initial state vector*/
.x = {
[0] = 50.0,
[2] = 10.0
/*Other values are zeros*/
},
/*State covariance components*/
.Up = {
/*
Here we have a unit upper triangular matrix.
We don't need to store ones, so, only upper parts of three columns are stored
*/
0, /*1st column*/
0,0, /*2nd column*/
0,0,0 /*3rd column*/
},
.Dp = {DP, DP, DP, DP}, /*Diagonal matrix is stored in a vector*/
/*State noise covariance components*/
.Uq = {
0,
0,0,
0,0,0
},
.Dq = {DX, DX, DX, DX},
/*Measurement noise covariance components*/
.Ur = {0},
.Dr = {DZ, DZ}
};
/*This is EKF structure definition*/
/* fx jfx hx jhx zrf nx nz rff qff memory */
yaflEKFBaseSt kf = YAFL_EKF_BASE_INITIALIZER(fx, jfx, hx, jhx, 0, NX, NZ, 0.0, 0.0, kf_memory);
/*
Arguments of initializer macro:
fx - state transition function pointer
jfx - state transition Jacobian function pointer
hx - measurement function pointer
jhx - measurement Jacobian function pointer
zrf - measurement Residual function pointer (needed to calculate the distance between forecast and measurement vectors)
nx - the dimension of state vector
nz - the dimension of measurement vector
rff - measurement noice covatiance forgetting factor
qff - process noice covatiance forgetting factor
memory - the name of a memory structure.
*/
So we've just declared the EKF memory structure, initiated it, we also declared and initiated EKF control block. Now we may want to filter some measurements:
extern volatile int some_condition;
extern yaflStatusEn get_some_measurement(yaflFloat * measurement_vector);
yaflFloat z[NZ]; /*Memory for measurement vectors*/
yaflStatusEn status;
while (some_condition)
{
/* This is Bierman filter predict step */
status = YAFL_EKF_BIERMAN_PREDICT(&kf);
if (YAFL_ST_OK != status)
{
/*Handle errors here*/
(void)status;
}
/*Now get one measurement vector*/
status = get_some_measurement(&z[0]);
if (YAFL_ST_OK != status)
{
/*Handle errors here*/
(void)status;
}
/*OK! We have a correct measurement at this point. Let's process it...*/
status = yafl_ekf_bierman_update(&kf, &z[0]);
if (YAFL_ST_OK != status)
{
/*Handle errors here*/
(void)status;
}
}
Yeah! Thats it! We've used Sequential Bierman filter to process our measurements.
Filter algorithms and data structures are declared in src/yafl.h. To power the filtering code we need some math functions which are declared in src/yafl_math.
Functions declared in src/yafl.h and src/yafl_math return result code, defined in yaflStatusEn
.
If everything is OK then YAFL_ST_OK = 0
is returned. If something went wrong the result code is actually a bit mask.
Here is the bit field list:
Warning bit fields:
YAFL_ST_MSK_REGULARIZED
- we had to do regularization to maintain the filter integrityYAFL_ST_MSK_GLITCH_SMALL
- a small error signal glitch detected: the error signal is large and its weigth is reduced. This field is used n Robust filters.YAFL_ST_MSK_GLITCH_LARGE
- a large error signal glitch detected, the error signal is too large and only its sign should be used for filter update. This field is used n Robust filters.YAFL_ST_MSK_ANOMALY
- an error signal anomaly detected, adaptive correction was done to handle it. This field is used in Adaptive filters.Error bit field values:
YAFL_ST_INV_ARG_1
- invalid argument 1YAFL_ST_INV_ARG_2
- invalid argument 2YAFL_ST_INV_ARG_3
- invalid argument 3...
YAFL_ST_INV_ARG_11
- invalid argument 11The execution of the YAFL code is stoped on the first error, so only one error may appear during YAFL function call.
We think that errors are actually unlikely to happen so every error check in our code is wrapped in YAFL_UNLIKELY
macro.
If this macro doesn't mean never and if some printf-like function oe macro is used in YAFL_LOG
macro, then you can get something like stacktrace in your console og log file if error happens.
On the other hand a single call to some YAFL function may yield many warning bits.
To disthiguish between warinings and errors YAFL_ST_ERR_THR
is used, e.g.:
if (YAFL_ST_OK != status)
{
if (YAFL_ST_ERR_THR >= status)
{
/*Handle warnings*/
(void)status;
}
else
{
/*Handle errors*/
(void)status;
}
}
We prefer using static memory allocation so filter structure initializers look like this:
/*yaflKalmanBaseSt is for internal usage only, so our code axemple is about EKF*/
yaflEKFBaseSt kf = YAFL_EKF_BASE_INITIALIZER(fx, jfx, hx, jhx, zrf, nx, nz, rff, qff, memory);
where:
fx
- state transition function pointerjfx
- state transition Jacobian function pointerhx
- measurement function pointerjhx
- measurement Jacobian function pointerzrf
- measurement Residual function pointer (needed to calculate the distance between forecast and measurement vectors)nx
- the dimension of state vectornz
- the dimension of measurement vectorrff
- measurement noice covatiance forgetting factorqff
- process noice covatiance forgetting factormemory
- the name of the memory structure.The filter control block does not store an data it has only pointers to data storage, so we must have som separate data storage called memory
.
Actually all filter control block structures in YAFL have only immutable data so we can declare them as constants if needed.
Functions fx
, jfx
, hx
, jhx
, must have prototypes which must be similar to:
yaflStatusEn fn(yaflKalmanBaseSt * self, yaflFloat *a, yaflFloat *b)
.
The function zrf
must have the prototype which must be similar to:
yaflStatusEn fn(yaflKalmanBaseSt * self, yaflFloat *result, yaflFloat *measurement, yaflFloat *forecast)
it is utilized to compute the error vectors as there are some cases when we can't use the Euclid distance.
As you can see all functions have yaflKalmanBaseSt * self
as their first parameter this is done to enable passing additional data to these functions.
So all these functions are "methods" of some "class" which has yaflKalmanBaseSt
as its "basic class". As you can see we are using OOP approach here.
Now let's talk about memory. We use mixins to declare filter memory pools so users can put more fields at their filter memory pools. This also enables us to do nested mixin macros calls while keeping memory pool strustures flat.
We used this paper to implement optional Q and R adaptive adjustments. Here are som notes on our implementation:
rff
to a small positive number e.g. 1e-4
.qff
to a small positive number e.g. 1e-4
.The basic type for all EKF control blocks is yaflEKFBaseSt
. It is initialized with YAFL_EKF_BASE_INITIALIZER
macro.
In the example above we gave the initial explanation of its work. Now let's go deeper.
We will use numpy code to explain stuff so you should learn something about numpy.
Here is the basic definition:
yaflStatusEn fx(yaflKalmanBaseSt * self, yaflFloat * new_x, yaflFloat * old_x)
{
(void)xz;
YAFL_CHECK(self, YAFL_ST_INV_ARG_1);
YAFL_CHECK(NX == self->Nx, YAFL_ST_INV_ARG_1);
YAFL_CHECK(new_x, YAFL_ST_INV_ARG_2);
YAFL_CHECK(old_x, YAFL_ST_INV_ARG_3);
/*
Here we must update state, numpy code is:
new_x = self.fx(old_x)
*/
return YAFL_ST_OK;
}
This definition applies to both EKF and UKF.
In EKF case new_x == old_x
so you can just do self.x = self.fx(self.x)
.
In UKF case new_x != old_x
.
Tbe basic definition is:
yaflStatusEn jfx(yaflKalmanBaseSt * self, yaflFloat * w, yaflFloat * x)
{
yaflInt i;
yaflInt nx;
yaflInt nx2;
YAFL_CHECK(self, YAFL_ST_INV_ARG_1);
YAFL_CHECK(NX == self->Nx, YAFL_ST_INV_ARG_1);
YAFL_CHECK(w, YAFL_ST_INV_ARG_2);
YAFL_CHECK(x, YAFL_ST_INV_ARG_3);
nx = self->Nx;
nx2 = nx * 2;
/* Here we must compute Jacobian of state transition function.
w has: nx rows and 2*nx columns so we will do:
w[:,0:nx] = self.jfx(self.x)
*/
return YAFL_ST_OK;
}
This definition is EKF specific.
Tbe basic definition is:
yaflStatusEn hx(yaflKalmanBaseSt * self, yaflFloat * y, yaflFloat * x)
{
YAFL_CHECK(self, YAFL_ST_INV_ARG_1);
YAFL_CHECK(NZ == self->Nz, YAFL_ST_INV_ARG_1);
YAFL_CHECK(y, YAFL_ST_INV_ARG_2);
YAFL_CHECK(x, YAFL_ST_INV_ARG_3);
/*
Here we must do:
y = self.hx(x)
*/
return YAFL_ST_OK;
}
This definition applies to both EKF and UKF.
In EKF case so you actually do self.y = self.hx(self.x)
.
For UKF x
and y
are state and measurement sigma points respectively.
The basic defeinition is:
yaflStatusEn jhx(yaflKalmanBaseSt * self, yaflFloat * h, yaflFloat * x)
{
yaflInt i;
yaflInt nx;
yaflInt nz;
YAFL_CHECK(self, YAFL_ST_INV_ARG_1);
YAFL_CHECK(NX == self->Nx, YAFL_ST_INV_ARG_1);
YAFL_CHECK(NZ == self->Nz, YAFL_ST_INV_ARG_1);
YAFL_CHECK(h, YAFL_ST_INV_ARG_2);
YAFL_CHECK(x, YAFL_ST_INV_ARG_3);
nx = self->Nx;
nz = self->Nz;
/* Here we will use a full H matrix which has nz rows and nx columns
We must do:
self.h = self.jhx(self.x)
*/
return YAFL_ST_OK;
}
This definition is EKF specific.
The basic definition is:
yaflStatusEn zrf(yaflKalmanBaseSt * self, yaflFloat *result, yaflFloat *a, yaflFloat *b)
{
yaflInt i;
yaflInt nz;
YAFL_CHECK(self, YAFL_ST_INV_ARG_1);
YAFL_CHECK(NZ == self->Nz, YAFL_ST_INV_ARG_1);
YAFL_CHECK(result, YAFL_ST_INV_ARG_2);
YAFL_CHECK(a, YAFL_ST_INV_ARG_2);
YAFL_CHECK(b, YAFL_ST_INV_ARG_3);
/*
Here we must do:
result = self.zrf(a, b)
for linear case:
result = a - b
*/
return YAFL_ST_OK;
}
This definition applies to both EKF and UKF.
In EKF case result == b == self.y
so you actually do self.y = self.zrf(measurement, self.y)
.
In UKF case result == self.y
, a == self.zp
and b
is some of measurement sigma points respectively.
Memory pools are declared using mixins:
typedef struct {
YAFL_EKF_BASE_MEMORY_MIXIN(NX, NZ); /* This is for EKF storage. */
/* You can declare som more storage here */
} someMemorySt;
Where:
NX
- A state vector size.NZ
- A measurement vector size.The most convenient way to initiate a filter memory is somthing like this:
someMemorySt memory =
{
/*Initial state vector*/
.x = {
/*Initiate x[0]...x[NX-1]*/
},
/*State covariance components*/
.Up = {
/*
Here we have a unit upper triangular matrix.
We don't need to store ones, so, only upper parts of three columns are stored
*/
0, /*1st column*/
0,0, /*2nd column*/
0,0,0, /*3rd column*/
/*...*/
0,0,0, /*...*/ 0 /*(NX - 1)-th column*/
},
/* 0 1 2 (NX - 1) elements*/
.Dp = {DP, DP, DP, /*...*/ DP}, /*Diagonal matrix is stored in a vector*/
/*State noise covariance components*/
.Uq = {
0, /*1st column*/
0,0, /*2nd column*/
0,0,0, /*3rd column*/
/*...*/
0,0,0, /*...*/ 0 /*(NX - 1)-th column*/
},
/* 0 1 2 (NX - 1) elements*/
.Dq = {DX, DX, DX, /*...*/ DX},
/*Measurement noise covariance components*/
.Ur = {
0, /*1st column*/
0,0, /*2nd column*/
0,0,0, /*3rd column*/
/*...*/
0,0,0, /*...*/ 0 /*(NZ - 1)-th column*/
},
/* 0 1 2 (NZ - 1) elements*/
.Dr = {DX, DX, DX, /*...*/ DX},
};
Basic EKF control block can be initialized with:
yaflEKFBaseSt kf = YAFL_EKF_BASE_INITIALIZER(fx, jfx, hx, jhx, zrf, nx, nz, rff, qff, memory);
Predict macros are:
YAFL_EKF_BIERMAN_PREDICT(self)
for Bierman filterYAFL_EKF_JOSEPH_PREDICT(self)
for Joseph sequential UD-factorized filterWhere self
is a pointer to a filter.
These macros call yaflStatusEn _yafl_ekf_predict_wrapper(yaflEKFBaseSt * self);
which is not supposed to be called directly by the user.
The example of filter predict call is:
status = YAFL_EKF_BIERMAN_PREDICT(&kf);
Update functions are:
yaflStatusEn yafl_ekf_bierman_update(yaflEKFBaseSt * self, yaflFloat *z);
for Bierman filteryaflStatusEn yafl_ekf_joseph_update(yaflEKFBaseSt * self, yaflFloat *z);
for Joseph sequential UD-factorized filterWhere:
self
is a pointer to a filter,z
is a pointer to a measurement vector.The example of filter predict call is:
status = yafl_ekf_bierman_update(&kf, &z[0]);
In these EKF variants H-infinity filter algorithm is used to mitigate the Kalman filter divergence. The Chi-square test is used to detect the filter divergence.
The filter control block type is:
typedef struct {
yaflEKFBaseSt base;
yaflFloat chi2;
} yaflEKFAdaptiveSt;
The memory mixin used is YAFL_EKF_BASE_MEMORY_MIXIN
.
The initilizer macro is:
YAFL_EKF_ADAPTIVE_INITIALIZER(fx, jfx, hx, jhx, zrf, nx, nz, rff, qff, chi2, memory)
As you can see it takes the same parameter list as YAFL_EKF_BASE_INITIALIZER
. the chi2
field is set to scipy.stats.chi2.ppf(0.999, 1)
Predict macros are:
YAFL_EKF_ADAPTIVE_BIERAMN_PREDICT(self)
for Bierman filterYAFL_EKF_ADAPTIVE_JOSEPH_PREDICT(self)
for Joseph sequential UD-factorized filterWhere self
is a pointer to a filter.
These macros call yaflStatusEn _yafl_ada_ekf_predict_wrapper(yaflEKFAdaptiveSt * self);
which is not supposed to be called directly by the user.
The example of filter predict call is:
status = YAFL_EKF_ADAPTIVE_BIERAMN_PREDICT(&kf);
Update functions are:
yaflStatusEn yafl_ekf_adaptive_bierman_update(yaflEKFAdaptiveSt * self, yaflFloat *z);
for Bierman filteryaflStatusEn yafl_ekf_adaptive_joseph_update(yaflEKFAdaptiveSt * self, yaflFloat *z);
for Joseph sequential UD-factorized filterWhere:
self
is a pointer to a filter,z
is a pointer to a measurement vector.The example of filter predict call is:
status = yafl_ekf_adaptive_bierman_update(&kf, &z[0]);
In these EKF variants generalized linear model is used to deal with measurement glithces and non-Gaussian noise.
The influence function and it's first derivative are used to mitigate glitches.
The functions which we have used for test are:
/*
The noise model is Normal with Poisson outliers.
See:
A.V. Chernodarov, G.I. Djandjgava and A.P. Rogalev (1999).
Monitoring and adaptive robust protection of the integrity of air data
inertial satellite navigation systems for maneuverable aircraft.
In: Proceedings of the RTO SCI International Conference on Integrated Navigation Systems,
held at "Electropribor", St. Petersburg, pp. 2111-10, RTO-MP-43 , NeuiIly-sur-Seine Cedex, France.
*/
yaflFloat psi(yaflKalmanBaseSt * self, yaflFloat normalized_error)
{
(void)self;
yaflFloat abs_ne = fabs(normalized_error);
if (3.0 >= abs_ne)
{
return normalized_error;
}
if (6.0 >= abs_ne)
{
/*Small glitch*/
return normalized_error / 3.0;
}
/*Large glitch*/
return (normalized_error >= 0.0) ? 1.0 : -1.0;
}
yaflFloat psi_dot(yaflKalmanBaseSt * self, yaflFloat normalized_error)
{
(void)self;
yaflFloat abs_ne = fabs(normalized_error);
if (3.0 >= abs_ne)
{
return 1.0;
}
if (6.0 >= abs_ne)
{
/*Small glitch*/
return 1.0 / 3.0;
}
/*Large glitch*/
return 0.0;
}
Where normalized error is error[i]/Dr[i]
.
Note that Dr
vector now holds square roots of corresponding dispersions
The filter control block type is:
typedef struct {
yaflEKFBaseSt base;
yaflKalmanRobFuncP g; /* g = -d(ln(pdf(y))) / dy */
yaflKalmanRobFuncP gdot; /* gdot = G = d(g) / dy */
} yaflEKFRobustSt;
The memory mixin used is YAFL_EKF_BASE_MEMORY_MIXIN
.
The initilizer macro is:
YAFL_EKF_ROBUST_INITIALIZER(fx, jfx, hx, jhx, zrf, g, gdot, nx, nz, rff, qff, memory)
Note that we have two additional arguments:
g
is influence functiongdot
is influence function first derivativeThe example for YAFL_EKF_ROBUST_INITIALIZER
call is:
yaflEKFRobustSt kf = YAFL_EKF_ROBUST_INITIALIZER(fx, jfx, hx, jhx, zrf, psi, psi_dot, nx, nz, rff, qff, memory);
Predict macros are:
YAFL_EKF_ROBUST_BIERAMN_PREDICT(self)
for Bierman filterYAFL_EKF_ROBUST_JOSEPH_PREDICT(self)
for Joseph sequential UD-factorized filterWhere self
is a pointer to a filter.
These macros call yaflStatusEn _yafl_rob_ekf_predict_wrapper(yaflEKFRobustSt * self);
which is not supposed to be called directly by the user.
The example of filter predict call is:
status = YAFL_EKF_ROBUST_BIERAMN_PREDICT(&kf);
Update functions are:
yaflStatusEn yafl_ekf_robust_bierman_update(yaflEKFRobustSt * self, yaflFloat *z);
for Bierman filteryaflStatusEn yafl_ekf_robust_joseph_update(yaflEKFRobustSt * self, yaflFloat *z);
for Joseph sequential UD-factorized filterWhere:
self
is a pointer to a filter,z
is a pointer to a measurement vector.The example of filter predict call is:
status = yafl_ekf_robust_bierman_update(&kf, &z[0]);
These EKF variants use both generalized linear model and adaptive correction.
The filter control block type is:
typedef struct {
yaflEKFRobustSt base;
yaflFloat chi2;
} yaflEKFAdaptiveRobustSt;
The memory mixin used is YAFL_EKF_BASE_MEMORY_MIXIN
.
The initilizer macro is:
YAFL_EKF_ADAPTIVE_ROBUST_INITIALIZER(fx, jfx, hx, jhx, zrf, g, gdot, nx, nz, rff, qff, chi2, memory)
As you can see it takes the same parameter list as YAFL_EKF_ROBUST_INITIALIZER
. the chi2
field is set to scipy.stats.chi2.ppf(0.997, 1)
Predict macros are:
YAFL_EKF_ADAPTIVE_ROBUST_BIERAMN_PREDICT(self)
for Bierman filterYAFL_EKF_ADAPTIVE_ROBUST_JOSEPH_PREDICT(self)
for Joseph sequential UD-factorized filterWhere self
is a pointer to a filter.
These macros call yaflStatusEn _yafl_ada_rob_predict_wrapper(yaflEKFAdaptiveRobustSt * self);
which is not supposed to be called directly by the user.
The example of filter predict call is:
status = YAFL_EKF_ADAPTIVE_ROBUST_BIERAMN_PREDICT(&kf);
Update functions are:
yaflStatusEn yafl_ekf_adaptive_robust_bierman_update(yaflEKFAdaptiveRobustSt * self, yaflFloat *z);
for Bierman filteryaflStatusEn yafl_ekf_adaptive_robust_joseph_update(yaflEKFAdaptiveRobustSt * self, yaflFloat *z);
for Joseph sequential UD-factorized filterWhere:
self
is a pointer to a filter,z
is a pointer to a measurement vector.The example of filter predict call is:
status = yafl_ekf_adaptive_robust_bierman_update(&kf, &z[0]);
The basic type for all UKF control blocks is yaflUKFBaseSt
.
The memory mixin used is YAFL_UKF_BASE_MEMORY_MIXIN
which is similar to other memory mixins in YAFL.
YAFL_UKF_BASE_INITIALIZER(points, points_methods, fx, xmf, xrf, hx, zmf, zrf, nx, nz, rff, memory)
It is quite similar to EKF base initializer but there are no Jacobian function pointers and there are som new parameters:
points
- a pointer to sigma point generator objectpoints_methods
- a pointer to sigma point generator vtablexmf
- a pointer to a state mean functionxrf
- a pointer to a state residual functionzmf
- a ponter to a measurement mean functionNote that we don't have qff
parameter since Q
adjustment leads to numerical instability of Unscented filter variants!
Sigma point generator is runtime extension to yaflUKFBaseSt
data type.
We desided to add such runtime extension to UKF types because user may
need to customize sigma point generation processes.
In YAFL_UKF_BASE_INITIALIZER
call we must pass a pointer to Sigma point
generator object and a pointer to corresponding vtable.
Vtable is an object of yaflUKFSigmaMethodsSt
type and it has two methods pointers to functions like:
yaflStatusEn wf(yaflUKFBaseSt * self);
which computes sigma points wegthsyaflStatusEn spgf(yaflUKFBaseSt * self);
which generates sigma pointsAs you can see the self
parameter has yaflUKFBaseSt
type, so these methods are actually UKF virtual methods.
On the other hand all sigma point generators have yaflUKFSigmaSt
base type which is:
typedef struct _yaflUKFSigmaSt {
yaflInt np; /* The number of sigma points */
yaflUKFSigmaAddP addf; /* Sigma point addition function */
} yaflUKFSigmaSt;
The actual addf
function is something like this:
yaflStatusEn my_cool_addf(yaflUKFBaseSt * self, yaflFloat * delta, yaflFloat * pivot, yaflFloat mult)
{
YAFL_CHECK(self, YAFL_ST_INV_ARG_1);
YAFL_CHECK(NX == self->Nx, YAFL_ST_INV_ARG_1);
YAFL_CHECK(delta, YAFL_ST_INV_ARG_2);
YAFL_CHECK(pivot, YAFL_ST_INV_ARG_3);
/*
Here we something like this:
delta = pivot + mult * delta;
*/
return YAFL_ST_OK;
}
It is used by sigma point generator to add deltas to initial state vector to get state sigma points in case when simple addition is not possible, e.g. in constrained UKF variants.
Currently we suppport only Van der Merwe sigma points generator, more planned.
To store sigma points we use corresponding mixins, e.g. YAFL_UKF_MERWE_MEMORY_MIXIN
, see later.
Van der Merwe sigma points generator have yaflUKFMerweSt
type.
The yaflUKFMerweSt
object can be defined as:
yaflUKFMerweSt my_sp_object = YAFL_UKF_MERWE_INITIALIZER(nx, addf, alpha, beta, kappa, memory);
Where:
nx
- is state vector sizealpha
, beta
, kappa
- are Van der Merwe sigma points parametersmemory
- is a name of UKF memory pool objectIn case uf UKF memory pool structure declaration should look like this:
typedef struct {
YAFL_UKF_BASE_MEMORY_MIXIN(NX, NZ);
YAFL_UKF_MERWE_MEMORY_MIXIN(NX, NZ);
/*Other fields*/
} myUKFMemorySt;
Vam der Merwe sigma point generator methods are in
const yaflUKFSigmaMethodsSt yafl_ukf_merwe_spm;
Julier sigma points generator have yaflUKFJulierSt
type.
The yaflUKFJulierSt
object can be defined as:
yaflUKFJulierSt my_sp_object = YAFL_UKF_JULIER_INITIALIZER(nx, addf, kappa, memory);
Where:
nx
- is state vector sizekappa
- is Julier scaling parametermemory
- is a name of UKF memory pool objectIn case uf UKF memory pool structure declaration should look like this:
typedef struct {
YAFL_UKF_BASE_MEMORY_MIXIN(NX, NZ);
YAFL_UKF_JULIER_MEMORY_MIXIN(NX, NZ);
/*Other fields*/
} myUKFMemorySt;
Julier sigma points generator methods are in
const yaflUKFSigmaMethodsSt yafl_ukf_julier_spm;
Mean and residual functions are used by Unscented transform in cases when simple weighted mean and Euclidian distance are unusable.
State residual functions should look like this:
yaflStatusEn xrf(yaflKalmanBaseSt * self, yaflFloat *result, yaflFloat *a, yaflFloat *b)
{
yaflInt i;
yaflInt nz;
YAFL_CHECK(self, YAFL_ST_INV_ARG_1);
YAFL_CHECK(NX == self->Nx, YAFL_ST_INV_ARG_1);
YAFL_CHECK(result, YAFL_ST_INV_ARG_2);
YAFL_CHECK(a, YAFL_ST_INV_ARG_3);
YAFL_CHECK(b, YAFL_ST_INV_ARG_4);
/*
Here we must do:
result = self.xrf(a, b)
for linear case:
result = a - b
*/
return YAFL_ST_OK;
}
State mean functions should look like this:
#define _UKF_SELF ((yaflUKFBaseSt *)self) /*Or you can pass yaflUKFBaseSt * self as first argument*/
yaflStatusEn xmf(yaflKalmanBaseSt * self, yaflFloat *result, yaflFloat *sigmas_x)
{
yaflInt nx;
yaflInt np;
YAFL_CHECK(self, YAFL_ST_INV_ARG_1);
nx = self->Nx;
YAFL_CHECK(NX == mx, YAFL_ST_INV_ARG_1);
YAFL_CHECK(_UKF_SELF->sp_info, YAFL_ST_INV_ARG_1);
YAFL_CHECK(_UKF_SELF->sp_meth, YAFL_ST_INV_ARG_1);
YAFL_CHECK(_UKF_SELF->sigmas_x, YAFL_ST_INV_ARG_1);
YAFL_CHECK(_UKF_SELF->wm, YAFL_ST_INV_ARG_1);
YAFL_CHECK(result, YAFL_ST_INV_ARG_2);
YAFL_CHECK(sigmas_x, YAFL_ST_INV_ARG_3);
np = _UKF_SELF->sp_info->np; /*Get number of sigmas*/
/*
Here we must do:
result = self.xmf(sigmas_x, self.wm)
for linear case:
result = self.wm.T.dot(sigmas_x)
*/
return YAFL_ST_OK;
}
Measurement mean functions should look like this:
yaflStatusEn zmf(yaflKalmanBaseSt * self, yaflFloat *result, yaflFloat *sigmas_z)
{
yaflInt nz;
yaflInt np;
YAFL_CHECK(self, YAFL_ST_INV_ARG_1);
nz = self->Nz;
YAFL_CHECK(NX == nz, YAFL_ST_INV_ARG_1);
YAFL_CHECK(_UKF_SELF->sp_info, YAFL_ST_INV_ARG_1);
YAFL_CHECK(_UKF_SELF->sp_meth, YAFL_ST_INV_ARG_1);
YAFL_CHECK(_UKF_SELF->sigmas_z, YAFL_ST_INV_ARG_1);
YAFL_CHECK(_UKF_SELF->wm, YAFL_ST_INV_ARG_1);
YAFL_CHECK(result, YAFL_ST_INV_ARG_2);
YAFL_CHECK(sigmas_z, YAFL_ST_INV_ARG_3);
np = _UKF_SELF->sp_info->np; /*Get number of sigmas*/
/*
Here we must do:
result = self.zmf(sigmas_z, self.wm)
for linear case:
result = self.wm.T.dot(sigmas_z)
*/
return YAFL_ST_OK;
}
Common UKF functions are:
static inline yaflStatusEn yafl_ukf_post_init(yaflUKFBaseSt * self);
/* and */
static inline yaflStatusEn yafl_ukf_gen_sigmas(yaflUKFBaseSt * self);
These functions are applicable to all UKF variants.
The yafl_ukf_post_init
must be called before any prediction or correction step.
The yafl_ukf_gen_sigmas
may be called between prediction and correction steps to make the behaviour of filters more "CLASSICAL".
The type of Bierman UKF control blocks is yaflUKFBaseSt
.
The memory mixin used is YAFL_UKF_BASE_MEMORY_MIXIN
which is similar to other memory mixins in YAFL.
The initializer used is YAFL_UKF_BASE_INITIALIZER
.
Predict and update functions are simmilar to their EKF counterparts.
The predict function is yafl_ukf_bierman_predict
.
The update function is yafl_ukf_bierman_update
.
The filter bootstrap may look this:
/*---------------------------------------------------------------------------*/
/*This is our filter memory structure*/
typedef struct {
YAFL_UKF_BASE_MEMORY_MIXIN(NX, NZ);
YAFL_UKF_MERWE_MEMORY_MIXIN(NX, NZ);
/*Other fields*/
} myUKFMemorySt;
/*Now we will initiate our memory*/
/*Some constants*/
#define DP (0.1)
#define DX (1.0e-6)
#define DZ (400)
myUKFMemorySt ukf_memory =
{
/*Initial state vector*/
.x = {
[0] = 50.0,
[2] = 10.0
/*Other values are zeros*/
},
/*State covariance components*/
.Up = {
/*
Here we have a unit upper triangular matrix.
We don't need to store ones, so, only upper parts of three columns are stored
*/
0, /*1st column*/
0,0, /*2nd column*/
0,0,0 /*3rd column*/
},
.Dp = {DP, DP, DP, DP}, /*Diagonal matrix is stored in a vector*/
/*State noise covariance components*/
.Uq = {
0,
0,0,
0,0,0
},
.Dq = {DX, DX, DX, DX},
/*Measurement noise covariance components*/
.Ur = {0},
.Dr = {DZ, DZ}
};
/*Sigma points generator stuff*/
extern yaflStatusEn my_cool_addf(yaflUKFBaseSt * self, yaflFloat * delta, yaflFloat * pivot, yaflFloat mult);
/* nx addf alpha beta kappa memory*/
yaflUKFMerweSt sp = YAFL_UKF_MERWE_INITIALIZER(NX, my_cool_addf, 0.1, 2., 0, ukf_memory);
/*Filter stuff*/
extern yaflStatusEn fx(yaflKalmanBaseSt * self, yaflFloat * new_x, yaflFloat * old_x);
extern yaflStatusEn xmf(yaflKalmanBaseSt * self, yaflFloat *result, yaflFloat *sigmas_x);
extern yaflStatusEn xrf(yaflKalmanBaseSt * self, yaflFloat *result, yaflFloat *a, yaflFloat *b);
extern yaflStatusEn hx(yaflKalmanBaseSt * self, yaflFloat * y, yaflFloat * x)
extern yaflStatusEn zmf(yaflKalmanBaseSt * self, yaflFloat *result, yaflFloat *sigmas_z);
extern yaflStatusEn zrf(yaflKalmanBaseSt * self, yaflFloat *result, yaflFloat *a, yaflFloat *b)
/* points points_metods fx xmf xrf hx zmf zrf nx nz rff memory */
yaflEKFBaseSt kf = YAFL_UKF_BASE_INITIALIZER(sp, yafl_ukf_merwe_spm, fx, xmf, xrf, hx, zmf, zrf, NX, NX, 0.0, ukf_memory);
The filter init sequence may look like this:
yaflStatusEn status;
status = yafl_ukf_post_init(&kf);
if (YAFL_ST_OK != status)
{
/*Handle errors here*/
(void)status;
}
The filter run may look like this:
extern volatile int some_condition;
extern yaflStatusEn get_some_measurement(yaflFloat * measurement_vector);
yaflFloat z[NZ]; /*Memory for measurement vectors*/
while (some_condition)
{
/* This is Bierman filter predict step */
status = yafl_ukf_bierman_predict(&kf);
if (YAFL_ST_OK != status)
{
/*Handle errors here*/
(void)status;
}
/*Now get one measurement vector*/
status = get_some_measurement(&z[0]);
if (YAFL_ST_OK != status)
{
/*Handle errors here*/
(void)status;
}
/*OK! We have a correct measurement at this point. Let's process it...*/
/*We have an option to generate sigma points manually at that point:*/
//status = yafl_ukf_gen_sigmas(&kf);
//if (YAFL_ST_OK != status)
//{
// /*Handle errors here*/
// (void)status;
//}
/*The filter update*/
status = yafl_ukf_bierman_update(&kf, &z[0]);
if (YAFL_ST_OK != status)
{
/*Handle errors here*/
(void)status;
}
}
The type of Adaptive Bierman UKF control blocks is yaflUKFAdaptivedSt
. The basic type for yaflUKFAdaptivedSt
is yaflUKFBaseSt
.
The memory mixin used is YAFL_UKF_BASE_MEMORY_MIXIN
which is similar to other memory mixins in YAFL.
The initializer used is YAFL_UKF_ADAPTIVE_INITIALIZER
which is simmilar to YAFL_UKF_BASE_INITIALIZER
.
Predict and update functions are simmilar to their Adaptive EKF counterparts.
The predict function is yafl_ukf_adaptive_bierman_predict
.
The update function is yafl_ukf_adaptive_bierman_update
.
The filter bootstap initialization and run are simmilar to Bierman UKF.
The type of Robust Bierman UKF control blocks is yaflUKFRobustSt
. The basic type for yaflUKFRobustSt
is yaflUKFBaseSt
.
The memory mixin used is YAFL_UKF_BASE_MEMORY_MIXIN
which is similar to other memory mixins in YAFL.
The initializer used is:
yaflEKFBaseSt kf = YAFL_UKF_ROBUST_INITIALIZER(points, points_methods, fx, xmf, xrf, hx, zmf, zrf, g, gdot, nx, nz, rff, memory);
where:
g
is influence functiongdot
is influence function first derivative. Yes, we are using derivatives here!So Robust Bierman UKF is simmilar to Robust Bierman EKF.
Predict and update functions are simmilar to their Adaptive EKF counterparts.
The predict function is yafl_ukf_robust_bierman_predict
.
The update function is yafl_ukf_robust_bierman_update
.
The filter bootstap initialization and run are simmilar to Bierman UKF whith the following exception:
extern yaflFloat psi(yaflKalmanBaseSt * self, yaflFloat normalized_error);
extern yaflFloat psi_dot(yaflKalmanBaseSt * self, yaflFloat normalized_error);
/*The filter is initiated differently from basic UKF:*/
/* points points_metods fx xmf xrf hx zmf zrf g gdot nx nz rff memory */
yaflEKFBaseSt kf = YAFL_UKF_ROBUST_INITIALIZER(sp, yafl_ukf_merwe_spm, fx, xmf, xrf, hx, zmf, zrf, psi, psi_dot, NX, NX, 0.0, ukf_memory);
The type of Adaptive robust Bierman UKF control blocks is yaflUKFAdaptiveRobustSt
. The basic type for yaflUKFAdaptiveRobustSt
is yaflUKFRobustSt
.
The memory mixin used is YAFL_UKF_BASE_MEMORY_MIXIN
which is similar to other memory mixins in YAFL.
The initializer used is YAFL_UKF_ADAPTIVE_ROBUST_INITIALIZER
which is simmilar to YAFL_UKF_ROBUST_INITIALIZER
.
Predict and update functions are simmilar to their Adaptive EKF counterparts.
The predict function is yafl_ukf_adaptive_robust_bierman_predict
.
The update function is yafl_ukf_adaptive_robust_bierman_update
.
The filter bootstap initialization and run are simmilar to Robust Bierman UKF.
The filter does not use recusions simmilar to sequential filters.
The type of UD-factorized UKF control blocks is yaflUKFSt
. The basic type for yaflUKFSt
is yaflUKFBaseSt
.
The memory mixin used is YAFL_UKF_MEMORY_MIXIN
which is similar to other memory mixins in YAFL.
The initializer used is YAFL_UKF_INITIALIZER
which is simmilar to YAFL_UKF_BASE_INITIALIZER
.
Predict and update functions are simmilar to their Bierman UKF counterparts.
The predict function is yafl_ukf_predict
.
The update function is yafl_ukf_update
.
The filter bootstap initialization and run are simmilar to Bierman UKF.
The filter does not use recusions simmilar to sequential filters.
The type of UD-factorized adaptive UKF control blocks is yaflUKFFullAdapiveSt
. The basic type for yaflUKFFullAdapiveSt
is yaflUKFSt
.
The memory mixin used is YAFL_UKF_MEMORY_MIXIN
which is similar to other memory mixins in YAFL.
The initializer used is: YAFL_UKF_INITIALIZER
/* points points_metods fx xmf xrf hx zmf zrf nx nz rff, chi2 memory */
yaflEKFBaseSt kf = YAFL_UKF_FULL_ADAPTIVE_INITIALIZER(sp, yafl_ukf_merwe_spm, fx, xmf, xrf, hx, zmf, zrf, NX, NX, rff, chi2, ukf_memory);
where chi2
must be set to scipy.stats.chi2.ppf(1.0 - some_small_number, NZ)
as we can not predict chi2
for specific nz
at compile time using C-preprocessor.
Predict and update functions are simmilar to their Bierman UKF counterparts.
The predict function is yafl_ukf_adaptive_predict
.
The update function is yafl_ukf_adaptive_update
.
The filter bootstap initialization and run are simmilar to UD-factorized UKF with exception of YAFL_UKF_FULL_ADAPTIVE_INITIALIZER
which needs chi2
value.
And we hope that you run YAFL on your MCUs and get some usefull results.