generate_jacob_theta_fem.cpp File Reference

File to generate Jacobian matrix and measurement vector for EKF (for femur) to calculate collar joint angles. More...

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

Include dependency graph for generate_jacob_theta_fem.cpp:

Functions
int	generate_jacobian_femur_theta (gsl_matrix *J, gsl_vector *state_vec)
	Generate Jacobian for current state for motion of femur collar.
int	generate_mes_vec_femur (gsl_vector *mes_vector, femur &fem, int frame_count)
	Generate measurement vector for the measurement equation of the EKF for calulating thetas.
int	h_measurement_femur_theta (gsl_vector *h, double theta1, double theta2, double theta3, double theta4, double theta5)
	Generate measurement vector for specific case, using elements of state vector.
int	h_femur_theta_encaps (gsl_vector *h, gsl_vector *state_vec)
	Generate predicted measurement vector using state-vector (encapsulator for h_measurement_femur())

---

Detailed Description

File to generate Jacobian matrix and measurement vector for EKF (for femur) to calculate collar joint angles.

---

Function Documentation

int generate_jacobian_femur_theta	(	gsl_matrix *	J,
		gsl_vector *	state_vec
	)

Generate Jacobian for current state for motion of femur collar.

We assume that the configuration is that with 6 plates for the femur, sensor locations given as that for readings done on aug 23, 2011. The calcuations are explained in a pdf explaining EKF, and obtained from the maple file jacob_femur_sensor.maple in the postprocess directory

All sensors need to have their data loaded into femur structure before running this function

Parameters:

[out]	J	10x5 jacobian matrix for function relating the state of the system (the collar joint angles to the measurement vector (distances between optical marker points)
[in]	state_vec	5x1 state vector containing [theta1,..., theta5]
[in]	fem	Femur structure containing all config data and measurements

Todo:

If possible, rewrite function to automate equations. ie. form equations based on fem structure & config file and use values on the fly to generate jacobian.

Referenced by run_kalman_femur_theta().

Here is the caller graph for this function:

int generate_mes_vec_femur	(	gsl_vector *	mes_vector,
		femur &	fem,
		int	frame_count
	)

Generate measurement vector for the measurement equation of the EKF for calulating thetas.

Parameters:

[out]	mes_vector	The 10x1 vector denoting output/measurement vector (size of vector already defined, hence we can make this function generic)
[in]	fem	Femur structure containing all config data and measurements
[in]	frame_count	Index to identify the time step/frame number for calculation

References sensor::mdata, and tibia::optical.

int h_femur_theta_encaps	(	gsl_vector *	h,
		gsl_vector *	state_vec
	)

Generate predicted measurement vector using state-vector (encapsulator for h_measurement_femur())

References h_measurement_femur_theta().

Referenced by run_kalman_femur_theta().

Here is the call graph for this function:

Here is the caller graph for this function:

int h_measurement_femur_theta	(	gsl_vector *	h,
		double	theta1,
		double	theta2,
		double	theta3,
		double	theta4,
		double	theta5
	)

Generate measurement vector for specific case, using elements of state vector.

Referenced by h_femur_theta_encaps().

Here is the caller graph for this function: