File to generate Jacobian matrix and measurement vector for EKF (for femur) More...

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

Include dependency graph for generate_jacob_fem.cpp:

Functions
int	generate_jacobian_femur (gsl_matrix J, gsl_vector state_vec, vector< vector< double > > wire_a_points, vector< double > thetas, vector< double > q)
	Generate Jacobian for current state for motion of femur collar.
int	generate_mes_vec_femur (gsl_vector mes_vector, femur &fem, int frame_count, int &wire_frame_count, gsl_vector tot_wire_len, vector< double > &q)
	Generate measurement vector for the measurement equation of the EKF.
int	h_measurement_femur (gsl_vector h, double sa, double sddot, double qa, double theta1, double theta2, double theta3, double theta4, double theta5, double a1, double a2, double a3, double a4, double a5, double a6)
	Generate measurement vector for specific case, using elements of state vector.
int	h_femur_encaps (gsl_vector h, gsl_vector state_vec, vector< vector< double > > wire_a_points, vector< double > thetas, vector< double > q)
	Generate predicted measurement vector using state-vector (encapsulator for h_measurement_femur())
int	jacob_set_tot (gsl_matrix H, double sa[3], double sddot[3], double qa[4], double theta1, double theta2, double theta3, double theta4, double theta5, double a1, double a2, double a3, double a4, double a5, double *a6)
	Generate measurement jacobian.

Detailed Description

File to generate Jacobian matrix and measurement vector for EKF (for femur)

Function Documentation

int generate_jacobian_femur	(	gsl_matrix *	J,
		gsl_vector *	state_vec,
		vector< vector< double > >	wire_a_points,
		vector< double >	thetas,
		vector< double >	q
	)

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	34x18 jacobian matrix for function relating the state of the system to the measurement vector
[in]	state_vec	18x1 state vector containing [s, s_dot, s_ddot, q, 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.

References gsl_vector2double(), jacob_set_tot(), and vectormat2double().

Referenced by run_kalman_femur().

Here is the call graph for this function:

Here is the caller graph for this function:

int generate_mes_vec_femur	(	gsl_vector *	mes_vector,
		femur &	fem,
		int	frame_count,
		int &	wire_frame_count,
		gsl_vector *	tot_wire_len,
		vector< double > &	q
	)

Generate measurement vector for the measurement equation of the EKF.

Parameters:

[out]	mes_vector	The 34x1 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
[in]	tot_wire_len	active wire calibs. i.e total wire length.

acceleration in local frame expressed as quaternion for multiplication

acceleration in global frame expressed as quaternion for multiplication

Todo:: find first wire connected to collar from "left", check if it's active or passive. assign

References tibia::accelerometers, tibia::accl_count, tibia::act_wire_count, tibia::active_wires, sensor::label_id, sensor::mdata, sensor::nb_pulley, tibia::opti_count, tibia::optical, tibia::pass_wire_count, and tibia::passive_wires.

Referenced by run_kalman_femur(), and run_kalman_femur_theta().

Here is the caller graph for this function:

int h_femur_encaps	(	gsl_vector *	h,
		gsl_vector *	state_vec,
		vector< vector< double > >	wire_a_points,
		vector< double >	thetas,
		vector< double >	q
	)

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

References gsl_vector2double(), h_measurement_femur(), and vectormat2double().

Referenced by run_kalman_femur().

Here is the call graph for this function:

Here is the caller graph for this function:

int h_measurement_femur	(	gsl_vector *	h,
		double *	sa,
		double *	sddot,
		double *	qa,
		double	theta1,
		double	theta2,
		double	theta3,
		double	theta4,
		double	theta5,
		double *	a1,
		double *	a2,
		double *	a3,
		double *	a4,
		double *	a5,
		double *	a6
	)

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

Referenced by h_femur_encaps().

Here is the caller graph for this function:

int jacob_set_tot	(	gsl_matrix *	H,
		double	sa[3],
		double	sddot[3],
		double	qa[4],
		double	theta1,
		double	theta2,
		double	theta3,
		double	theta4,
		double	theta5,
		double *	a1,
		double *	a2,
		double *	a3,
		double *	a4,
		double *	a5,
		double *	a6
	)

Generate measurement jacobian.

The quaternion is expressed as Rodrigues parameters to prevent problems of solutions with quaternion magnitude different from 1

Referenced by generate_jacobian_femur().

Here is the caller graph for this function:

Functions

Detailed Description

Function Documentation