#include "kalman_filter.h"
#include "../ptp_defs.h"
#include <math.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <flexptp_options.h>

Macros
#define	SIGMA_THETA_SQUARED (1E-16)

#define	SIGMA_GAMMA_SQUARED (1E-12)

#define	SIGMA_CT_SQUARED (1E-10)

#define	FAST_TUNING_THRESHOLD (500E-09)

#define	FAST_TUNING_COEFFICIENT (0.2)

#define	CALM_TUNING_COEFFICIENT (0.01)

#define	MTX_ELEMENTWISE(ctx)

#define	VEC_ELEMENTWISE(ctx)

#define	SQR(x) ((x) * (x))

Typedefs
typedef double	mtx[2][2]

typedef double	vec[2]

Functions
static void	mtx_copy (mtx dst, mtx src)

static void	mtx_add (mtx r, mtx a, mtx b)

static void	mtx_sub (mtx r, mtx a, mtx b)

static void	mtx_scale (mtx r, double c, mtx a)

static void	mtx_dot (vec r, mtx m, vec v)

static void	mtx_mul (mtx r, mtx a, mtx b)

static void	mtx_transp (mtx r, mtx m)

static double	mtx_det (mtx m)

static void	mtx_inverse (mtx r, mtx m)

static void	mtx_print (mtx m)

static void	mtx_zero (mtx m)

static void	mtx_unit (mtx m)

static void	vec_copy (vec r, vec v)

static void	vec_add (vec r, vec a, vec b)

static void	vec_sub (vec r, vec a, vec b)

static void	vec_zero (vec v)

static void	init_variances ()

void	kalman_filter_init ()

void	kalman_filter_deinit ()

void	kalman_filter_reset ()

static void	insert_DT (double DT)

float	kalman_filter_run (int32_t dt, PtpServoAuxInput *pAux)

Variables
static mtx	A

static mtx	B

static mtx	H

static vec	u

static mtx	Q

static mtx	K

static mtx	P

static mtx	P_pri

static mtx	R

static mtx	I

static vec	z

static vec	x_pri

static vec	x

static double	sigma_theta_squared

static double	sigma_gamma_squared

static double	sigma_theta_m_squared

static uint64_t	cycle

static int32_t	dt_prev

Macro Definition Documentation

◆ CALM_TUNING_COEFFICIENT

#define CALM_TUNING_COEFFICIENT (0.01)

Definition at line 34 of file kalman_filter.c.

◆ FAST_TUNING_COEFFICIENT

#define FAST_TUNING_COEFFICIENT (0.2)

Definition at line 33 of file kalman_filter.c.

◆ FAST_TUNING_THRESHOLD

#define FAST_TUNING_THRESHOLD (500E-09)

Definition at line 32 of file kalman_filter.c.

◆ MTX_ELEMENTWISE

#define MTX_ELEMENTWISE ( ctx )

Value:

    for (uint8_t i = 0; i < 2; i++) {     \
        for (uint8_t j = 0; j < 2; j++) { \
            ctx                           \
        }                                 \
    }

Definition at line 41 of file kalman_filter.c.

◆ SIGMA_CT_SQUARED

#define SIGMA_CT_SQUARED (1E-10)

Definition at line 27 of file kalman_filter.c.

◆ SIGMA_GAMMA_SQUARED

#define SIGMA_GAMMA_SQUARED (1E-12)

Definition at line 23 of file kalman_filter.c.

◆ SIGMA_THETA_SQUARED

#define SIGMA_THETA_SQUARED (1E-16)

A Kalman-filter based servo. This module implements the Kalman-filter introduced in the paper: 'Performance Analysis of Kalman-Filter-Based Clock Synchronization in IEEE 1588 Networks' by Giada Gorgi and Claudio Narduzzi (https://ieeexplore.ieee.org/document/5934411)

Definition at line 19 of file kalman_filter.c.

◆ SQR

#define SQR ( x ) ((x) * (x))

Definition at line 137 of file kalman_filter.c.

◆ VEC_ELEMENTWISE

#define VEC_ELEMENTWISE ( ctx )

Value:

    for (uint8_t i = 0; i < 2; i++) { \
        ctx                           \
    }

Definition at line 48 of file kalman_filter.c.

Typedef Documentation

◆ mtx

typedef double mtx[2][2]

Definition at line 38 of file kalman_filter.c.

◆ vec

typedef double vec[2]

Definition at line 39 of file kalman_filter.c.

Function Documentation

◆ init_variances()

static void init_variances ( )

static

Definition at line 245 of file kalman_filter.c.

◆ insert_DT()

static void insert_DT ( double DT )

static

Definition at line 286 of file kalman_filter.c.

◆ kalman_filter_deinit()

void kalman_filter_deinit ( )

Deinitialize the Kalman-filter.

Definition at line 260 of file kalman_filter.c.

◆ kalman_filter_init()

void kalman_filter_init ( )

A Kalman-filter based servo. This module implements the Kalman-filter introduced in the paper: 'Performance Analysis of Kalman-Filter-Based Clock Synchronization in IEEE 1588 Networks' by Giada Gorgi and Claudio Narduzzi (https://ieeexplore.ieee.org/document/5934411) Initialize the Kalman-filter.

Definition at line 251 of file kalman_filter.c.

◆ kalman_filter_reset()

void kalman_filter_reset ( )

Reset the Kalman-filter.

Definition at line 266 of file kalman_filter.c.

◆ kalman_filter_run()

float kalman_filter_run	(	int32_t	dt,
		PtpServoAuxInput *	pAux
	)

Run the Kalman-filter.

Parameters

dt	time error in nanoseconds
pAux	auxiliary synchronization cycle context data

Definition at line 298 of file kalman_filter.c.

◆ mtx_add()

static void mtx_add	(	mtx	r,
		mtx	a,
		mtx	b
	)

static

Definition at line 58 of file kalman_filter.c.

◆ mtx_copy()

static void mtx_copy	(	mtx	dst,
		mtx	src
	)

static

Definition at line 53 of file kalman_filter.c.

◆ mtx_det()

static double mtx_det ( mtx m )

static

Definition at line 90 of file kalman_filter.c.

◆ mtx_dot()

static void mtx_dot	(	vec	r,
		mtx	m,
		vec	v
	)

static

Definition at line 73 of file kalman_filter.c.

◆ mtx_inverse()

static void mtx_inverse	(	mtx	r,
		mtx	m
	)

static

Definition at line 94 of file kalman_filter.c.

◆ mtx_mul()

static void mtx_mul	(	mtx	r,
		mtx	a,
		mtx	b
	)

static

Definition at line 78 of file kalman_filter.c.

◆ mtx_print()

static void mtx_print ( mtx m )

static

Definition at line 103 of file kalman_filter.c.

◆ mtx_scale()

static void mtx_scale	(	mtx	r,
		double	c,
		mtx	a
	)

static

Definition at line 68 of file kalman_filter.c.

◆ mtx_sub()

static void mtx_sub	(	mtx	r,
		mtx	a,
		mtx	b
	)

static

Definition at line 63 of file kalman_filter.c.

◆ mtx_transp()

static void mtx_transp	(	mtx	r,
		mtx	m
	)

static

Definition at line 85 of file kalman_filter.c.

◆ mtx_unit()

static void mtx_unit ( mtx m )

static

Definition at line 112 of file kalman_filter.c.

◆ mtx_zero()

static void mtx_zero ( mtx m )

static

Definition at line 107 of file kalman_filter.c.

◆ vec_add()

static void vec_add	(	vec	r,
		vec	a,
		vec	b
	)

static

Definition at line 122 of file kalman_filter.c.

◆ vec_copy()

static void vec_copy	(	vec	r,
		vec	v
	)

static

Definition at line 117 of file kalman_filter.c.

◆ vec_sub()

static void vec_sub	(	vec	r,
		vec	a,
		vec	b
	)

static

Definition at line 127 of file kalman_filter.c.

◆ vec_zero()

static void vec_zero ( vec v )

static

Definition at line 132 of file kalman_filter.c.

Variable Documentation

◆ A

mtx A

static

Definition at line 143 of file kalman_filter.c.

◆ B

mtx B

static

Definition at line 144 of file kalman_filter.c.

◆ cycle

uint64_t cycle

static

Definition at line 163 of file kalman_filter.c.

◆ dt_prev

int32_t dt_prev

static

Definition at line 164 of file kalman_filter.c.

◆ H

mtx H

static

Definition at line 145 of file kalman_filter.c.

◆ I

mtx I

static

Definition at line 152 of file kalman_filter.c.

◆ K

mtx K

static

Definition at line 148 of file kalman_filter.c.

◆ P

mtx P

static

Definition at line 149 of file kalman_filter.c.

◆ P_pri

mtx P_pri

static

Definition at line 150 of file kalman_filter.c.

◆ Q

mtx Q

static

Definition at line 147 of file kalman_filter.c.

◆ R

mtx R

static

Definition at line 151 of file kalman_filter.c.

◆ sigma_gamma_squared

double sigma_gamma_squared

static

Definition at line 158 of file kalman_filter.c.

◆ sigma_theta_m_squared

double sigma_theta_m_squared

static

Definition at line 159 of file kalman_filter.c.

◆ sigma_theta_squared

double sigma_theta_squared

static

Definition at line 157 of file kalman_filter.c.

◆ u

vec u

static

Definition at line 146 of file kalman_filter.c.

◆ x

vec x

static

Definition at line 155 of file kalman_filter.c.

◆ x_pri

vec x_pri

static

Definition at line 154 of file kalman_filter.c.

◆ z

vec z

static

Definition at line 153 of file kalman_filter.c.

Macros

Typedefs

Functions

Variables

Macro Definition Documentation

◆ CALM_TUNING_COEFFICIENT

◆ FAST_TUNING_COEFFICIENT

◆ FAST_TUNING_THRESHOLD

◆ MTX_ELEMENTWISE

◆ SIGMA_CT_SQUARED

◆ SIGMA_GAMMA_SQUARED

◆ SIGMA_THETA_SQUARED

◆ SQR

◆ VEC_ELEMENTWISE

Typedef Documentation

◆ mtx

◆ vec

Function Documentation

◆ init_variances()

◆ insert_DT()

◆ kalman_filter_deinit()

◆ kalman_filter_init()

◆ kalman_filter_reset()

◆ kalman_filter_run()

◆ mtx_add()

◆ mtx_copy()

◆ mtx_det()

◆ mtx_dot()

◆ mtx_inverse()

◆ mtx_mul()

◆ mtx_print()

◆ mtx_scale()

◆ mtx_sub()

◆ mtx_transp()

◆ mtx_unit()

◆ mtx_zero()

◆ vec_add()

◆ vec_copy()

◆ vec_sub()

◆ vec_zero()

Variable Documentation

◆ A

◆ B

◆ cycle

◆ dt_prev

◆ H

◆ I

◆ K

◆ P

◆ P_pri

◆ Q

◆ R

◆ sigma_gamma_squared

◆ sigma_theta_m_squared

◆ sigma_theta_squared

◆ u

◆ x

◆ x_pri

◆ z