EKF1 Class Reference

EKF1 is an implementation of a state estimator for JUMP sensors. More...

Inheritance diagram for EKF1:

Public Member Functions
function	EKF1 ()
	Constructor of the EKF1 class.
function	run (in obj, in options)
	filters and smooths the given data
function	exportTuning (in obj, in filename)
	exports the tuning parameters of the filter to a JSON file
function	importTuning (in obj, in filename)
	imports the tuning parameters of the filter from a JSON file

Public Attributes
Constant Property	n = 13
Constant Property	m = 13
Property	dt
	sampling time (default 1/200 s)
Property	zv
	zero velocity flag (1 x N)
Measurement Vector
attributes regarding the measurement vector
Property	N
	number of observations
Property	z_acc
	accelerometer measurements (3 x N) [m/s^2]
Property	z_vel
	velocity measurements (3 x N)
Property	z_gyr
	gyroscope measurements (3 x N) [rad/s]
Property	z_pos
	position pseudo measurements (3 x N) [m]
Property	z_yaw
	yaw pseudo measurements (1 x N) [rad]
Property	z
	measurement vector (m x N)
State Vector
attributes regarding the estimated state vectors
Property	x_fm
	state vector, forward, a priori (n x N)
Property	x_fp
	state vector, forward, a posteriori (n x N)
Property	x_bp
	state vector, backward, a posteriori (n x N)
Initial State Vector
attributes regarding the initial state vector
Property	q_0
	initial irientation quaternion
Property	v_0
	initial velocity
Property	p_0
	initial position
Property	b_w_0
	initial gyroscope bias
Property	x_0
	initial state vector (n x 1)
State Covariance
attributes regarding the state covariance matrix
Property	P_fm
	state covariance matrix, forward, a priori (n x n x N)
Property	P_fp
	state covariance matrix, forward, a posteriori (n x n x N)
Property	P_bp
	state covariance matrix, backward, a posteriori (n x n x N)
Initial State Covariance
attributes regarding the initial state covariance matrix
Property	P_0_q
	initial orientation standard deviation (default 5e-4)
Property	P_0_v
	initial velocity standard deviation (default 1e-7 m/s)
Property	P_0_p
	initial position standard deviation (default 1e-7 m)
Property	P_0_b_w
	initial gyroscope bias standard deviation (default 5e-3 rad/s)
Property	P_0
	initial state covariance matrix (n x n)
Measurement Noise
attributes regarding the measurement noise
Property	var_acc_xy
	accelerometer variance, x- and y-axis (default 0.045 m/s^2)
Property	var_acc_z
	accelerometer variance, z-axis (default 0.07 m/s^2)
Property	var_vel_ZUPT
	variance of velocity 'measurements' during ZUPTs (default 1e-6)
Property	var_gyr
	gyroscope variance (default 1.22e-3 rad/s)
Property	var_pos_x
	variance of x-position 'measurements' during ZUPTs (default 1e-6 m)
Property	var_pos_y
	variance of y-position 'measurements' during ZUPTs (default 1e-6 m)
Property	var_pos_z
	variance of z-position 'measurements' during ZUPTs (default 1e-6 m)
Property	var_yaw_pseudo
	variance of yaw pseudo-measurements (default TODO:)
Property	R
	measurement noise covariance matrix (m x m)
Process Noise
attributes regarding the process noise Note the process noise covariance matrix is calculated for each iteration based on the current state vector (see Q_cov)
Property	sigma_w_p
	angular velocity standard deviation (default 876e-6 rad/s)
Property	var_a
	acceleration variance (default 0.045 m/s^2)
Property	var_b_w
	gyroscope bias variance (default 1e-9 rad/s)

Private Member Functions
function	forward (in obj)
	forward pass of the EKF1 filter
function	backward (in obj)
	backward pass of the EKF1 filter
function	f_fun (in obj, in x_this, in z_this, in z_next, in zv)
	calculates the state transition function for the EKF1 filter
function	F_jac (in obj, in w)
	calculates the jacobian of the state transition function
function	Q_cov (in obj, in x)
	calculates the process noise covariance matrix

Static Private Member Functions
static function	H_jac (in x)
	calculates the jacobian of the measurement function from a given state vector x
static function	h_fun (in x)
	calculates the measurement function from a given state vector
static function	getPreprocessedAcc (in acc_this, in acc_next, in q_this, in q_next, in zv)
	calculates the preprocessed acceleration measurement
static function	getPreprocessedGyro (in gyro_this, in gyro_next, in bias)
	calculates the preprocessed gyro measurement
static function	displayResults (in x_fp, in x_bp, in zv, in options)
	displays the results of the EKF1 filter

Detailed Description

EKF1 is an implementation of a state estimator for JUMP sensors.

This class is an implementation of an Extended Kalman Filter (EKF) based on this derivation.

Warning

this implementation is changed from the original derivation. if you want to use the original implementation, use the version of the commit 4fac0d658d the changes are:

• the accelerometer bias is not estimated
• the accelerometer sensitivity is not estimated
• the measurement update of the accelerometer is changed

% example code
ekf = EKF1();
 
% set measurements
ekf.z_acc = acc;
ekf.z_gyr = gyr;
 
% set zero velocity flag
ekf.zv = zv;
 
% run the filter
[smoothed, filtered] = ekf.run();

Warning

all values regarding angles must be given in radians

Copyright

see the file LICENSE in the root directory of the repository

Definition at line 35 of file EKF1.m.

Constructor & Destructor Documentation

EKF1()

function EKF1

(

)

Constructor of the EKF1 class.

constructs an instance of the EKF1 class with the default values

Member Function Documentation

backward()

function backward ( in obj )

private

backward pass of the EKF1 filter

this function performs the backward pass of the EKF1 filter

Parameters

obj	instance of the EKF1 class

displayResults()

static function displayResults ( in x_fp, in x_bp, in zv, in options )

staticprivate

displays the results of the EKF1 filter

this function displays the results of the EKF1 filter

Parameters

x_fp	filtered state vector
x_bp	smoothed state vector
zv	zero velocity flag
options
options.monitor	monitor to display the results

exportTuning()

function exportTuning	(	in	obj,
		in	filename )

exports the tuning parameters of the filter to a JSON file

this function exports the tuning parameters of the filter to a JSON file

Parameters

obj	instance of the EKF1 class (implicit)
filename	name of the JSON file (full or relative path, default 'tuning.json')

Return values

status

1 if the file was written successfully, 0 otherwise

Note

The following parameters are exported:

• initial covariance
• measurement noise
• process noise

f_fun()

function f_fun ( in obj, in x_this, in z_this, in z_next, in zv )

private

calculates the state transition function for the EKF1 filter

this function calculates the state transition, also called the state prediction/extrapolation.

Parameters

x_this	state vector at time k
z_this	measurement vector at time k
z_next	measurement vector at time k+1
zv	zero velocity flag at time k

Return values

x_next	state vector at time k+1
w	preprocessed angular velocity

F_jac()

function F_jac ( in obj, in w )

private

calculates the jacobian of the state transition function

this function calculates the jacobian of the state transition function.

Parameters

w	preprocessed angular velocity at time k

Return values

F	jacobian of the state transition function at time k

forward()

function forward ( in obj )

private

forward pass of the EKF1 filter

this function performs the forward pass of the EKF1 filter

Parameters

obj	instance of the EKF1 class

getPreprocessedAcc()

static function getPreprocessedAcc ( in acc_this, in acc_next, in q_this, in q_next, in zv )

staticprivate

calculates the preprocessed acceleration measurement

this function calculates the preprocessed acceleration measurement as explained in the derivation. The acceleration measurements are preprocessed to get the bias free mean acceleration without gravity between two samples.

Parameters

acc_this	acceleration measurement at time k
acc_next	acceleration measurement at time k+1
q_this	orientation quaternion at time k
q_next	orientation quaternion at time k+1
zv	zero velocity flag at time k

Return values

acc	preprocessed acceleration

getPreprocessedGyro()

static function getPreprocessedGyro ( in gyro_this, in gyro_next, in bias )

staticprivate

calculates the preprocessed gyro measurement

this function calculates the preprocessed gyro measurement as explained in the derivation. The gyro measurements are preprocessed to get the bias free mean angular velocity between two samples.

Parameters

gyro_this	gyro measurement at time k
gyro_next	gyro measurement at time k+1
bias	bias of the gyroscope at time k
zv	zero velocity flag at time k (not used at the moment)

Return values

w	preprocessed angular velocity

Note

since a zero angular velocity leads to zeros in the F matrix which leads to zero entries in the covariance matrix, the differentiation cases are disabled. in some later implementations (e.g. UKF or particle filter approach) this could be enabled again.

h_fun()

static function h_fun ( in x )

staticprivate

calculates the measurement function from a given state vector

Parameters

x	state vector at time k

Return values

h	measurement vector at time k

H_jac()

static function H_jac ( in x )

staticprivate

calculates the jacobian of the measurement function from a given state vector x

Parameters

x	state vector at time k

Return values

H	jacobian of the measurement function at time k

importTuning()

function importTuning	(	in	obj,
		in	filename )

imports the tuning parameters of the filter from a JSON file

this function imports the tuning parameters of the filter from a JSON file

Parameters

obj	instance of the EKF1 class (implicit)
filename	name of the JSON file (full or relative path)

Return values

status

1 if the file was read successfully, 0 otherwise

Note

the function does only import the parameters that are present in the JSON file. if a parameter is not present, the existing value is kept. therefore, no check is performed if all parameters are present.

Q_cov()

function Q_cov ( in obj, in x )

private

calculates the process noise covariance matrix

this function calculates the process noise covariance matrix

Parameters

x	state vector at time k

Return values

Q	process noise covariance matrix

run()

function run	(	in	obj,
		in	options )

filters and smooths the given data

this function creates all needed vectors and matrices and also checks the input data. it then filters the data and smoothes the data

Parameters

options

options for the filter

Return values

smoothed	smoothed state vector
filtered	filtered state vector

this function creates all needed vectors and matrices and also checks the input data. it then filters the data and smoothes the data

Member Data Documentation

b_w_0

Property b_w_0

initial gyroscope bias

Definition at line 127 of file EKF1.m.

dt

Property dt

sampling time (default 1/200 s)

Definition at line 50 of file EKF1.m.

m

Constant Property m = 13

Definition at line 43 of file EKF1.m.

n

Constant Property n = 13

Definition at line 41 of file EKF1.m.

N

Property N

number of observations

Definition at line 62 of file EKF1.m.

p_0

Property p_0

initial position

Definition at line 123 of file EKF1.m.

P_0

Property P_0

initial state covariance matrix (n x n)

Remarks

based on experimental tuning

Definition at line 180 of file EKF1.m.

P_0_b_w

Property P_0_b_w

initial gyroscope bias standard deviation (default 5e-3 rad/s)

Remarks

based on experimental tuning

Definition at line 175 of file EKF1.m.

P_0_p

Property P_0_p

initial position standard deviation (default 1e-7 m)

Remarks

based on experimental tuning

Definition at line 170 of file EKF1.m.

P_0_q

Property P_0_q

initial orientation standard deviation (default 5e-4)

Remarks

based on experimental tuning

Definition at line 160 of file EKF1.m.

P_0_v

Property P_0_v

initial velocity standard deviation (default 1e-7 m/s)

Remarks

based on experimental tuning

Definition at line 165 of file EKF1.m.

P_bp

Property P_bp

state covariance matrix, backward, a posteriori (n x n x N)

Definition at line 149 of file EKF1.m.

P_fm

Property P_fm

state covariance matrix, forward, a priori (n x n x N)

Definition at line 141 of file EKF1.m.

P_fp

Property P_fp

state covariance matrix, forward, a posteriori (n x n x N)

Definition at line 145 of file EKF1.m.

q_0

Property q_0

initial irientation quaternion

Definition at line 115 of file EKF1.m.

R

Property R

measurement noise covariance matrix (m x m)

Definition at line 230 of file EKF1.m.

sigma_w_p

Property sigma_w_p

angular velocity standard deviation (default 876e-6 rad/s)

Remarks

based on allan variance analysis

Note

as the prediction is based on the measurement, the measurement noise applies. but since the prediction is based on the preprocessed angular velocity which is the mean angular velocity between two samples, the standard deviation is divided by sqrt(2) to get the standard deviation of the mean angular velocity.

Definition at line 248 of file EKF1.m.

v_0

Property v_0

initial velocity

Definition at line 119 of file EKF1.m.

var_a

Property var_a

acceleration variance (default 0.045 m/s^2)

Remarks

based on experimental tuning

Definition at line 253 of file EKF1.m.

var_acc_xy

Property var_acc_xy

accelerometer variance, x- and y-axis (default 0.045 m/s^2)

Remarks

based on allan variance analysis

Definition at line 191 of file EKF1.m.

var_acc_z

Property var_acc_z

accelerometer variance, z-axis (default 0.07 m/s^2)

Remarks

based on allan variance analysis

Definition at line 196 of file EKF1.m.

var_b_w

Property var_b_w

gyroscope bias variance (default 1e-9 rad/s)

Remarks

based on experimental tuning

Note

the value from the allan variance analysis seems to be too high. the value is set to a lower value based on experimental tuning.

Definition at line 260 of file EKF1.m.

var_gyr

Property var_gyr

gyroscope variance (default 1.22e-3 rad/s)

Remarks

based on allan variance analysis

Definition at line 206 of file EKF1.m.

var_pos_x

Property var_pos_x

variance of x-position 'measurements' during ZUPTs (default 1e-6 m)

Remarks

based on experimental tuning

Definition at line 211 of file EKF1.m.

var_pos_y

Property var_pos_y

variance of y-position 'measurements' during ZUPTs (default 1e-6 m)

Remarks

based on experimental tuning

Definition at line 216 of file EKF1.m.

var_pos_z

Property var_pos_z

variance of z-position 'measurements' during ZUPTs (default 1e-6 m)

Remarks

based on experimental tuning

Definition at line 221 of file EKF1.m.

var_vel_ZUPT

Property var_vel_ZUPT

variance of velocity 'measurements' during ZUPTs (default 1e-6)

Remarks

based on experimental tuning

Definition at line 201 of file EKF1.m.

var_yaw_pseudo

Property var_yaw_pseudo

variance of yaw pseudo-measurements (default TODO:)

Remarks

based on experimental tuning

Definition at line 226 of file EKF1.m.

x_0

Property x_0

initial state vector (n x 1)

Definition at line 131 of file EKF1.m.

x_bp

Property x_bp

state vector, backward, a posteriori (n x N)

Definition at line 105 of file EKF1.m.

x_fm

Property x_fm

state vector, forward, a priori (n x N)

Definition at line 97 of file EKF1.m.

x_fp

Property x_fp

state vector, forward, a posteriori (n x N)

Definition at line 101 of file EKF1.m.

z

Property z

measurement vector (m x N)

Definition at line 87 of file EKF1.m.

z_acc

Property z_acc

accelerometer measurements (3 x N) [m/s^2]

Definition at line 65 of file EKF1.m.

z_gyr

Property z_gyr

gyroscope measurements (3 x N) [rad/s]

Definition at line 75 of file EKF1.m.

z_pos

Property z_pos

position pseudo measurements (3 x N) [m]

Definition at line 79 of file EKF1.m.

z_vel

Property z_vel

velocity measurements (3 x N)

Note

these are usually all set to zero as the velocity is applied as a pseudo measurement during ZUPTs

Definition at line 71 of file EKF1.m.

z_yaw

Property z_yaw

yaw pseudo measurements (1 x N) [rad]

Definition at line 83 of file EKF1.m.

zv

Property zv

zero velocity flag (1 x N)

Definition at line 54 of file EKF1.m.

---

The documentation for this class was generated from the following file:

• classes/EKF1.m