ekf_func.cpp File Reference

File with functions necessary to perform necessary EKF steps. More...

#include <iostream>
#include <fstream>
#include "ekf.h"

Include dependency graph for ekf_func.cpp:

Functions
int	ekf_predict_step (gsl_vector *state_vec, gsl_matrix *P, gsl_matrix *A, gsl_matrix *W, gsl_matrix *Q, gsl_vector *state_vec_pred, gsl_matrix *P_pred)
	Estimate new state vector using system model.
int	ekf_kalman_gain (gsl_matrix *P_pred, gsl_matrix *H, gsl_matrix *V, gsl_matrix *R, gsl_matrix *K)
	Calculate Kalman gain.
int	ekf_update_estimate (gsl_vector *state_vec_up, gsl_matrix *K, gsl_vector *mes_vector, gsl_vector *state_vec_pred, gsl_vector *mes_predict)
	Update state vector estimate using Kalman gain.
int	ekf_update_cov (gsl_matrix *P_up, gsl_matrix *K, gsl_matrix *H, gsl_matrix *P_pred)
	Update error covariance estimate using Kalman gain.

---

Detailed Description

File with functions necessary to perform necessary EKF steps.

---

Function Documentation

int ekf_kalman_gain	(	gsl_matrix *	P_pred,
		gsl_matrix *	H,
		gsl_matrix *	V,
		gsl_matrix *	R,
		gsl_matrix *	K
	)

Calculate Kalman gain.

Parameters:

[in]	P_pred	Estimated error cov
[in]	H	measurement jacbian
[in]	V	measurement noise jacobian
[in]	R	measurement variance matrix
[out]	K	Kalman gain

References gsl_inverse().

Referenced by run_kalman_femur(), run_kalman_femur_theta(), and run_kalman_tib_low_theta().

Here is the call graph for this function:

Here is the caller graph for this function:

int ekf_predict_step	(	gsl_vector *	state_vec,
		gsl_matrix *	P,
		gsl_matrix *	A,
		gsl_matrix *	W,
		gsl_matrix *	Q,
		gsl_vector *	state_vec_pred,
		gsl_matrix *	P_pred
	)

Estimate new state vector using system model.

Parameters:

[in]	state_vec	Initial state
[in]	P	initial state error covariance
[in]	A	process matrix
[in]	W	process noise jacobian
[in]	Q	process noise cov
[out]	state_vec_pred	estimated state
[out]	P_pred	estimated error covariance

Referenced by run_kalman_femur(), run_kalman_femur_theta(), and run_kalman_tib_low_theta().

Here is the caller graph for this function:

int ekf_update_cov	(	gsl_matrix *	P_up,
		gsl_matrix *	K,
		gsl_matrix *	H,
		gsl_matrix *	P_pred
	)

Update error covariance estimate using Kalman gain.

Referenced by run_kalman_femur(), run_kalman_femur_theta(), and run_kalman_tib_low_theta().

Here is the caller graph for this function:

int ekf_update_estimate	(	gsl_vector *	state_vec_up,
		gsl_matrix *	K,
		gsl_vector *	mes_vector,
		gsl_vector *	state_vec_pred,
		gsl_vector *	mes_predict
	)

Update state vector estimate using Kalman gain.

Referenced by run_kalman_femur(), run_kalman_femur_theta(), and run_kalman_tib_low_theta().

Here is the caller graph for this function: