Data Structures | Defines | Functions
postprocess.h File Reference

File with declarations for post-processing functions. More...

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_multifit_nlin.h>
#include <gsl/gsl_deriv.h>
#include <gsl/gsl_fft_real.h>
#include <gsl/gsl_fft_halfcomplex.h>
#include "lowerleg_struct.h"
#include "ekf.h"
#include "matrix_tools.h"
Include dependency graph for postprocess.h:
This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Data Structures

struct  angle_data
 Structure to hold data for joint angle calculations. More...

Defines

#define PI   3.14159

Functions

int load_collar_calibs (char *foldername, vector< vector< double > > &Hprev, vector< vector< double > > &Pvals, vector< vector< double > > &qvals, vector< double > &firstlink, vector< vector< double > > &Plate)
 Load Calibrated Collar Data: This function loads the information regarding calibrated collars into vectors for use in postprocessing.
int load_opti_data (char *filename, vector< vector< double > > &datapoints, vector< string > &point_labels)
 Load Optical Marker Data.
int load_collar_opti_data (vector< vector< double > > &datapoints, vector< string > &point_labels, vector< vector< int > > point_flag, tibia &tib)
 Load Optical Marker data for all optical sensors on collar into tibia structure.
int load_accl_data (char *filename, int num_accls, int num_accl_entries, vector< vector< double > > &datapoints)
 Load Accelerometer Data for Postprocessing.
int load_collar_accl_data (char *filename, tibia &tib, femur &fem)
 Load Accelerometer data for all Accelerometers on leg into tibia structure.
int load_collar_active_data (vector< vector< double > > datapoints, tibia &tib, femur &fem)
 Load Active wire data for all active wires on leg into tibia and femur structure.
int load_active_data (char *filename, int num_act, vector< vector< double > > &datapoints)
 Load Active wire Data for Postprocessing.
int load_passive_data (char *filename, int num_act, vector< vector< double > > &datapoints)
 Load Passive wire Data for Postprocessing.
int load_collar_passive_data (vector< vector< double > > all_pass_data, tibia &tib, femur &fem)
 Load Passive wire data for all passive wires on leg into tibia and femur structure.
int load_all_measures (tibia &tib, femur &fem, vector< vector< double > > &frame_refs)
 Load all sensor data into leg structure.
int solve_gsl_lm (gsl_multifit_function_fdf *f, gsl_vector_view xv, double precision, int nbre_iter, gsl_multifit_fdfsolver *s, gsl_matrix *covar)
void print_results_lm (gsl_multifit_fdfsolver *s, size_t n, size_t p, int status, gsl_matrix *covar)
 Print Least Square result.
void gsl_rot_to_rodrigues (gsl_matrix *Rot, gsl_vector *rodrig)
void gsl_rodrigues_to_rot (gsl_vector *rodrig, gsl_matrix *Rot)
void encaps_vector2rot (double *vrot, double *Rot)
void gramSchmidt (gsl_vector *v1, gsl_vector *v2, gsl_vector *v3, gsl_matrix *T)
void rotX (double alpha_rad, gsl_matrix *Rot)
 Rotation matrix about axis X, angle in radians.
void rotY (double alpha_rad, gsl_matrix *Rot)
 Rotation matrix about axis Y, angle in radians.
void rotZ (double alpha_rad, gsl_matrix *Rot)
 Rotation matrix about axis Z, angle in radians.
void rotEuler (double X, double Y, double Z, gsl_matrix *Rot)
void gsl_vector2Rot_matrix (gsl_matrix *Rot, gsl_vector *v)
void gsl_Rot_matrix2vector (gsl_matrix *Rot, gsl_vector *v)
void mat_passage_3pts (gsl_vector *v1, gsl_vector *v2, gsl_vector *v3, gsl_matrix *T)
int gsl_hom_matrix (gsl_vector *origin, gsl_matrix *Rot, gsl_matrix *H)
 homogeneous transformation matrix
void free_acc (double *accl, double *quat)
 Subtract gravitational acceleration from accl readings.
void quat_multiply (gsl_vector *M1, gsl_vector *M2, gsl_vector *ans0)
 Multiply quaternions.
void quat_rotate (gsl_vector *quat1, gsl_vector *vect, gsl_vector *ans1)
 Rotate vector by quaternion.
void quat_invert (gsl_vector *quat)
void get_quaternion_avg (vector< vector< double > > quat_datas, gsl_vector *quat_avg)
 Get the average quaternion.
void rodrigues_to_quat (gsl_vector *quat_rod, gsl_vector *quat)
 Convert quaterion in rodrigues formulation to normal quaternion.
int calc_joint_angle_err (double angle_rad, double &DH_mat, double &Mat_loc, double &Ptloc, double &Ptglob, double &err)
int calc_j_angle_wrap (const gsl_vector *angle_rad, struct angle_data *angleD, gsl_vector *err)
 Calculate joint angle.
int jnt_angle_jacobian (double theta, double DH_mat[], double Mat_loc[], double Ptloc[3], double J[])
int jnt_angle_jacob_wrap (const gsl_vector *angle_rad, struct angle_data *angle_d, gsl_matrix *J)
int calc_jnt_angle_jacob_wrap (const gsl_vector *angle_rad, struct angle_data *angle_d, gsl_vector *err, gsl_matrix *J)
int calc_joint_ang (struct angle_data *angle_d, double theta_init, double precision, int nb_iter, double &theta_sol)
int calc_link_frame_opti (tibia &tib, string linkname, int fr_num, gsl_matrix *M)
 Calculate transformation matrix relating base link frame to global frame using optical marker data.
int calc_trans_mat (double Ploc[], double Pglob[], gsl_matrix *H)
 Given coordinates of 3 points in 2 frames, calculate transformation matrix relating them.
int DHtoFix (vector< double > &firstlink, gsl_matrix *DHF)
 calc first link matrix.
int DH_hayati (double beta, double alpha, double a, double theta, gsl_matrix *DHayati)
 Calculate transformation matrix using Hayati representation of DH parameters.
int gsl_pt_quat_matrix (vector< double > &Pvals, vector< double > &qvals, gsl_matrix *MatOP)
int get_link_number (collar &col, string linkname)
 Get the link number. ie plate number from the leftmost plate.
int calc_transf_matrix (gsl_matrix *Matlink, int linknumber, vector< vector< double > > &Hprev, vector< vector< double > > &Pvals, vector< vector< double > > &qvals, vector< double > &firstlink)
 Calculate overall transformation matrix, from first link (left-most) to the current link (specified by link number).
int clean_opti_data (vector< vector< double > > &datapoints, vector< vector< int > > &point_flag, vector< string > &point_labels)
 Clean Optical Marker Data and re-assign labels.
double getdist (vector< double > &basepoint, vector< double > &newpoint)
 Get distance between basepoint and newpoint.
int getmaxmin (vector< double > &currpt, vector< double > &max_pt, vector< double > &min_pt)
 Redefine diagonal points of box so that currpt is inside it.
int checkinbox (vector< double > &currpt, vector< double > &max_pt, vector< double > &min_pt)
 Check if currpt is inside box defined by diagonal points max_pt and min_pt.
double getcang (vector< double > &oldpoint, vector< double > &basepoint, vector< double > &newpoint)
void extract_data_point (vector< vector< double > > &datapoints, int variable_id, double data[])
 Extract single column of data from from 2d vector of datapoints.
int gsl_get_fourier_coeffs (double data[], gsl_vector *f_data, int num_frames, gsl_fft_real_wavetable *real, gsl_fft_real_workspace *work)
 Return a gsl_vector containing complex coeffs obtained from the Fourier transform.
int get_fourier_dominant_estimate (double data[], gsl_vector *f_data, int num_dominant_freq)
 Get the first k dominant frequencies in Fourier representation, and set all others to zero.
void gsl_filter_data (double data[], int num_frames, int num_dominant_freq)
void gsl_low_pass_fourier (double data[], gsl_vector *f_data, int num_freq)
void gsl_low_pass_filter_data (double data[], int num_frames, int num_freq)
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_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 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 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.
int load_mario_frame_opti_data (vector< vector< double > > &datapoints, vector< string > &point_labels, vector< vector< double > > &frame)
 Load the frame reference point coordinates.
int calc_mario_frame_opti (gsl_matrix *H, vector< vector< double > > &frame_mario, vector< vector< double > > &frame)
 Calculate the transformation matrix to convert from optical system to MARIONET frame of ref.
int convert_opti_to_mario (tibia &tib, gsl_matrix *H)
 Convert optical marker data in tibia structure to MARIONET reference based coordinate system.
int run_kalman_femur (femur &fem, vector< vector< double > > wire_A_points, gsl_vector *tot_wire_len, vector< vector< double > > &state, vector< double > thetas)
 Run the EKF on the femur.
int first_local_extrema (double *data, int num_iter, int frame_start)
 Find the first local extrema in the vector of doubles.
int opti_first_extrema (vector< vector< double > > &datapoints, int num_markers)
int first_local_extrema_vec (vector< vector< double > > data, int index, int index_step)
 Find the first local extrema in the vector of doubles.
double adj_labview_tmstamp (double tmstamp)
 Change from Labview epoch to UTC epoch.
int sync_opti_active (vector< vector< double > > &active_wire_data, vector< vector< double > > &optical_data, int num_opti_markers)
 Sync optical data with active_wire system.
int filter_sync_passive_active (vector< vector< double > > active_wire_data, vector< vector< double > > &passive_wire_data)
 Filter and sync passive wire data with active wire data.
int read_all_wire_bases (vector< vector< double > > &wire_bases)
int set_theta_err_cov (gsl_matrix *Q, int num_theta)
int set_initial_estimate (gsl_vector *state_estimate)
int set_process_mat (gsl_matrix *A, double deltaT)
int set_proc_cov_mat (gsl_matrix *Q)
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())
int run_kalman_femur_theta (femur &fem, vector< vector< double > > &state)
 Run the EKF on the femur.
int filter_accl (vector< vector< double > > &accl_data)
int accl_mario_orient (double *quat_mario, double *quat)
 Calculate the transformation to convert from accelerometer's earth-global frame to MARIONET frame of ref.
int generate_jacobian_tib_low_theta (gsl_matrix *J, gsl_vector *state_vec)
 Generate Jacobian for current state for motion of femur collar.
int generate_mes_vec_tib_low (gsl_vector *mes_vector, tibia &tib, int frame_count)
 Generate measurement vector for the measurement equation of the EKF for calulating thetas.
int h_measurement_tib_low_theta (gsl_vector *h, double theta1, double theta2, double theta3, double theta4)
 Generate measurement vector for specific case, using elements of state vector.
int h_tib_low_theta_encaps (gsl_vector *h, gsl_vector *state_vec)
 Generate predicted measurement vector using state-vector (encapsulator for h_measurement_tib_low())
int run_kalman_tib_low_theta (tibia &tib, vector< vector< double > > &state)
 Run the EKF on the femur.

Detailed Description

File with declarations for post-processing functions.

This file includes function definitions for those functions that will be needed for post-processing the data, and combining it to obtain useful results regarding motion, position and orientation of collar and so on.

Define Documentation

#define PI   3.14159

Function Documentation

int accl_mario_orient ( double *  quat_mario,
double *  quat 
)

Calculate the transformation to convert from accelerometer's earth-global frame to MARIONET frame of ref.

The function calculates the transformation that converts data in accelerometer system's reference frame (which expresses orientation with respect to the earth-fixed global frame) to the co-ordinates expressed in terms of reference system fixed to the MARIONET.

Parameters:
[in]quat_marioQuaternion describing orientation of MARIONET w.r.t earth fixed system
[in,out]quatQuaternion describing orientation of acclerometer w.r.t earth fixed system. The function calculates the quaternion w.r.t MARIONET and stores it back here.

References quat_multiply().

Referenced by load_accl_data().

Here is the call graph for this function:

Here is the caller graph for this function:

double adj_labview_tmstamp ( double  tmstamp)

Change from Labview epoch to UTC epoch.

Labview uses a different epoch for timestamps unlike UTC/Unix timestamps. This functions adjusts the Labview timestamps by adding the UTC value of the epoch 1/1/1904, 0:0:0 to Labview value.

Parameters:
[in,out]tmpstampNumber of seconds elapsed since epoch

Referenced by load_passive_data().

Here is the caller graph for this function:

int calc_j_angle_wrap ( const gsl_vector *  angle_rad,
struct angle_data *  angle_d,
gsl_vector *  err 
)

Calculate joint angle.

Wrapper function that calls the GSL Least Squares Solver and the joint angle error function

References calc_joint_angle_err(), angle_data::DH_mat, angle_data::Mat_loc, angle_data::Ptglob, and angle_data::Ptloc.

Referenced by calc_joint_ang().

Here is the call graph for this function:

Here is the caller graph for this function:

int calc_jnt_angle_jacob_wrap ( const gsl_vector *  angle_rad,
struct angle_data *  angle_d,
gsl_vector *  err,
gsl_matrix *  J 
)

References calc_joint_angle_err(), angle_data::DH_mat, jnt_angle_jacobian(), angle_data::Mat_loc, angle_data::Ptglob, and angle_data::Ptloc.

Referenced by calc_joint_ang().

Here is the call graph for this function:

Here is the caller graph for this function:

int calc_joint_ang ( struct angle_data *  angle_d,
double  theta_init,
double  precision,
int  nb_iter,
double &  theta_sol 
)
int calc_joint_angle_err ( double  angle_rad,
double &  DH_mat,
double &  Mat_loc,
double &  Ptloc,
double &  Ptglob,
double &  err 
)
int calc_link_frame_opti ( tibia &  tib,
string  linkname,
int  fr_num,
gsl_matrix *  M 
)

Calculate transformation matrix relating base link frame to global frame using optical marker data.

This function reads data from the config file and 3d tracking data obtained from optical markers to calculate the 4x4 transformation matrix that relates the link fixed frame to the global frame. The function takes in the structure tibia, which contains config data and the measured data, and the name of the link which is to be considered the base.

Since the OptiTrack system records data at a frame rate of 100Hz and stores it referencing the frame number, the frame number is also taken in as input.

Parameters:
[in]tibThe tibia structure holding all info
[in]linknameThe name of the link that is to be treated as the base link
[in]fr_numThe frame number for which the measured data is taken for calculations
[out]MThe 4x4 transformation matrix such that Ploc = M*pglob where, plocal are the coordinates of a point in the link frame, and pglob are the coordinates of the point in global frame.

number of sensors on the plate linkname

References calc_trans_mat(), sensor::LINK_label, MAX_OPTI_PER_LINK, sensor::mdata, tibia::opti_count, tibia::optical, sensor::xa, sensor::ya, and sensor::za.

Here is the call graph for this function:

int calc_mario_frame_opti ( gsl_matrix *  H,
vector< vector< double > > &  frame_mario,
vector< vector< double > > &  frame 
)

Calculate the transformation matrix to convert from optical system to MARIONET frame of ref.

The function calculates the transformation matrix that converts data in optical system's reference frame to the co-ordinates expressed in terms of reference system fixed to the MARIONET.

Points are stored in the file in the order of: Ox, Oy, O

Parameters:
[in,out]H4x4 matrix that transform coordinates in optical frame to marionet frame
[in]frame_mario2d vector containing the 3 coordinates of ref points in mario frame
[in]frame2d vector containing the 3 coordinates of ref points in optical frame

References calc_trans_mat(), and vectormat2double().

Referenced by load_all_measures().

Here is the call graph for this function:

Here is the caller graph for this function:

int calc_trans_mat ( double  Ploc[],
double  Pglob[],
gsl_matrix *  H 
)

Given coordinates of 3 points in 2 frames, calculate transformation matrix relating them.

Parameters:
[in]Ploc9x1 array with coordinates of 3 points in local frame, concatanated together.
[in]Pglob9x1 array with coordinates of 3 points in global frame, concatanated together.
[out]H4x4 transformation matrix such that plocal = H*pglobal, where plocal and pglobal are the co-ordinates expressed as homogenous 4x1 vectors

References gramSchmidt().

Referenced by calc_link_frame_opti(), and calc_mario_frame_opti().

Here is the call graph for this function:

Here is the caller graph for this function:

int calc_transf_matrix ( gsl_matrix *  Matlink,
int  linknumber,
vector< vector< double > > &  Hprev,
vector< vector< double > > &  Pvals,
vector< vector< double > > &  qvals,
vector< double > &  firstlink 
)

Calculate overall transformation matrix, from first link (left-most) to the current link (specified by link number).

Calculate the matrix to transform local co-ordinates in link/plate frame i (from left) to co-ordinates expressed in the left-most frame of the collar.

References DH_hayati(), DHtoFix(), and gsl_pt_quat_matrix().

Referenced by calc_pts_in_collar_base_frame().

Here is the call graph for this function:

Here is the caller graph for this function:

int checkinbox ( vector< double > &  currpt,
vector< double > &  max_pt,
vector< double > &  min_pt 
)

Check if currpt is inside box defined by diagonal points max_pt and min_pt.

Referenced by clean_opti_data().

Here is the caller graph for this function:

int clean_opti_data ( vector< vector< double > > &  datapoints,
vector< vector< int > > &  point_flag,
vector< string > &  point_labels 
)

Clean Optical Marker Data and re-assign labels.

This program analyses the Motion Capture Data stored in the vectors (created by the load_opti_data function) and reassigns labels to the correct point trajectory. This is necessary since the ARENA software tends to fail at recognizing points for their correct names and labels them either as "Unlabeled" points or assigns correct labels.

This function will scan the data framewise for each point (labeled and unlabeled) and assign the closest recognized point (under 10 mm)

WE ASSUME THAT FIRST 100 FRAMES ARE CORRECTLY IDENTIFIED!

Parameters:
[in,out]datapointsVector containing point data.
[out]point_flagVector containing flag = 1 if point is reliable, 0 if not. Same size as datapoints vector
[in]point_labelsLabels assigned to the points by the ARENA software

Number of points identified automatically by the software

Number of "real" markers labeled for the rigid body.

Number of frames of data stored

Flag indicates whether closest point method is applicable or not. 0 indicates closest point approach is to be used. 1 indicates that closest point approach will not work (since previous point is either "bad" or masked).

threshold for distance between point in frame i and frame i+1.

threshold for point velocity difference in frame i and i+1

References checkinbox(), getcang(), getdist(), and getmaxmin().

Referenced by load_all_measures(), and main().

Here is the call graph for this function:

Here is the caller graph for this function:

int convert_opti_to_mario ( tibia &  tib,
gsl_matrix *  H 
)

Convert optical marker data in tibia structure to MARIONET reference based coordinate system.

Parameters:
[in,out]tibTibia (or femur) structure containing all optical data points
[in]HTransformation matrix to convert coordinates in optical frame to MARIONET frame

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

Referenced by load_all_measures().

Here is the caller graph for this function:

int DH_hayati ( double  beta,
double  alpha,
double  a,
double  theta,
gsl_matrix *  DHayati 
)

Calculate transformation matrix using Hayati representation of DH parameters.

References gsl_hom_matrix(), rotX(), rotY(), and rotZ().

Referenced by calc_transf_matrix().

Here is the call graph for this function:

Here is the caller graph for this function:

int DHtoFix ( vector< double > &  firstlink,
gsl_matrix *  DHF 
)

calc first link matrix.

References gsl_hom_matrix(), rotX(), and rotZ().

Referenced by calc_collar_angles(), and calc_transf_matrix().

Here is the call graph for this function:

Here is the caller graph for this function:

void encaps_vector2rot ( double *  vrot,
double *  Rot 
)

References gsl_matrix2double(), and gsl_vector2Rot_matrix().

Here is the call graph for this function:

void extract_data_point ( vector< vector< double > > &  datapoints,
int  variable_id,
double  data[] 
)

Extract single column of data from from 2d vector of datapoints.

The input 2d vector datapoints contains data for n variables, over num_frame number of frames. datapoints[j][i] contains the value of i^th variable for frame j. This functions copies all values in datapoints[*][i] for given i into a array of doubles.

Referenced by filter_accl(), filter_sync_passive_active(), main(), and opti_first_extrema().

Here is the caller graph for this function:

int filter_accl ( vector< vector< double > > &  accl_data)

References extract_data_point(), and gsl_low_pass_filter_data().

Referenced by load_collar_accl_data().

Here is the call graph for this function:

Here is the caller graph for this function:

int filter_sync_passive_active ( vector< vector< double > >  active_wire_data,
vector< vector< double > > &  passive_wire_data 
)

Filter and sync passive wire data with active wire data.

Change the timestamps from Labview format to UTC time, filter out the noise and discard all data taken before active_wire system has begun, and downsample from 1Khz to approx 100 Hz (synchronizing with active data)

Parameters:
[in]active_wire_dataActive wire data needed to synchronize passive data
[in,out]passive_wire_dataPassive wire data to remove spurious noise.

References extract_data_point(), and gsl_low_pass_filter_data().

Referenced by load_all_measures().

Here is the call graph for this function:

Here is the caller graph for this function:

int first_local_extrema ( double *  data,
int  num_iter,
int  frame_start 
)

Find the first local extrema in the vector of doubles.

Parameters:
[in]dataArray containing noise free version of data

Referenced by opti_first_extrema().

Here is the caller graph for this function:

int first_local_extrema_vec ( vector< vector< double > >  data,
int  index,
int  index_step 
)

Find the first local extrema in the vector of doubles.

Parameters:
[in]dataVector containing noise free version of data, along with timestamps in column 0 of the vector
[in]indexThe index of the column to be used to find extrema
[in]index_step

Referenced by sync_opti_active().

Here is the caller graph for this function:

void free_acc ( double *  accl,
double *  quat 
)

Subtract gravitational acceleration from accl readings.

Parameters:
[in,out]acclAcceleration vector. The gravity-free component is stored back
[in]quatQuaternion describing orientation of acclerometer w.r.t earth fixed system

References quat_rotate().

Referenced by load_accl_data().

Here is the call graph for this function:

Here is the caller graph for this function:

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]J34x18 jacobian matrix for function relating the state of the system to the measurement vector
[in]state_vec18x1 state vector containing [s, s_dot, s_ddot, q, theta1,..., theta5]
[in]femFemur 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_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]J10x5 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_vec5x1 state vector containing [theta1,..., theta5]
[in]femFemur 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_jacobian_tib_low_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 5 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_thetas_tibia_low.maple in the postprocess directory

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

Parameters:
[out]J14x4 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_vec4x1 state vector containing [theta1,..., theta4]
[in]femFemur 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_tib_low_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_vectorThe 10x1 vector denoting output/measurement vector (size of vector already defined, hence we can make this function generic)
[in]femFemur structure containing all config data and measurements
[in]frame_countIndex to identify the time step/frame number for calculation

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

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_vectorThe 34x1 vector denoting output/measurement vector (size of vector already defined, hence we can make this function generic)
[in]femFemur structure containing all config data and measurements
[in]frame_countIndex to identify the time step/frame number for calculation
[in]tot_wire_lenactive 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 generate_mes_vec_tib_low ( gsl_vector *  mes_vector,
tibia &  tib,
int  frame_count 
)

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

Parameters:
[out]mes_vectorThe 14x1 vector denoting output/measurement vector (size of vector already defined, hence we can make this function generic)
[in]femFemur structure containing all config data and measurements
[in]frame_countIndex to identify the time step/frame number for calculation

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

Referenced by run_kalman_tib_low_theta().

Here is the caller graph for this function:

int get_fourier_dominant_estimate ( double  data[],
gsl_vector *  f_data,
int  num_dominant_freq 
)

Get the first k dominant frequencies in Fourier representation, and set all others to zero.

This function takes the fourier representation obtained from gsl_get_fourier_coeffs function, identifies the top k dominant frequencies (k specified as input), and sets all others to zero. Thus, the function ignores all contributions from all non-dominant frequencies.

References gsl_vector2double().

Referenced by gsl_filter_data().

Here is the call graph for this function:

Here is the caller graph for this function:

int get_link_number ( collar &  col,
string  linkname 
)

Get the link number. ie plate number from the leftmost plate.

References NB_LINKS_MAX, collar::shape_left, and collar::shape_right.

Referenced by calc_pts_in_collar_base_frame().

Here is the caller graph for this function:

void get_quaternion_avg ( vector< vector< double > >  quat_datas,
gsl_vector *  quat_avg 
)

Get the average quaternion.

Since quaternions are not regular vectors, but rather representations of orientation, an average quaternion cannot just be obtained by taking a weighted mean. This function implements the work done by paper by F. Landis Merkley (DOI: 10.2514/1.28949) to calculate the average quaternion

double getcang ( vector< double > &  oldpoint,
vector< double > &  basepoint,
vector< double > &  newpoint 
)

get cosine of angle between vectors sharing 1 point. head of 1 vector is tail of the other. and use it to calculate the acceleration magnitude.

\[ \left | accl \right | = \left |v_2 - v_1 \right | \]

\[ \left | accl \right | = \sqrt{((v_2 cos(\theta)- v_1)^2 + (v_2 sin(\theta))^2)} \]

\[ \left | accl \right | = \sqrt{(v_2^2 +v_1^2 - 2*v_1 \dot v_2)} \]

References getdist().

Referenced by clean_opti_data().

Here is the call graph for this function:

Here is the caller graph for this function:

double getdist ( vector< double > &  basepoint,
vector< double > &  newpoint 
)

Get distance between basepoint and newpoint.

Referenced by clean_opti_data(), and getcang().

Here is the caller graph for this function:

int getmaxmin ( vector< double > &  currpt,
vector< double > &  max_pt,
vector< double > &  min_pt 
)

Redefine diagonal points of box so that currpt is inside it.

Referenced by clean_opti_data().

Here is the caller graph for this function:

void gramSchmidt ( gsl_vector *  v1,
gsl_vector *  v2,
gsl_vector *  v3,
gsl_matrix *  R 
)

Construct matrix such that v2-v1 is x-axis, v3-v2-v1 is the x-y-z plane. R converts local coordinates of frame fixed to v1,v2,v3 to global coords.

References gsl_vector_cross_product3D(), and gsl_vector_norme().

Referenced by calc_trans_mat(), and mat_passage_3pts().

Here is the call graph for this function:

Here is the caller graph for this function:

void gsl_filter_data ( double  data[],
int  num_frames,
int  num_dominant_freq 
)

References get_fourier_dominant_estimate(), and gsl_get_fourier_coeffs().

Here is the call graph for this function:

int gsl_get_fourier_coeffs ( double  data[],
gsl_vector *  f_data,
int  num_frames,
gsl_fft_real_wavetable *  real,
gsl_fft_real_workspace *  work 
)

Return a gsl_vector containing complex coeffs obtained from the Fourier transform.

Parameters:
[in]data[]1d array containing the data to be filtered.
Returns:
Gsl_vector of size 2*num_frames with 2*i'th element containing real part of i'th Fourier coefficient, and 2*i+1'th element containing imaginary part of the i'th coefficient.

Referenced by gsl_filter_data(), and gsl_low_pass_filter_data().

Here is the caller graph for this function:

int gsl_hom_matrix ( gsl_vector *  P,
gsl_matrix *  Rot,
gsl_matrix *  H 
)

homogeneous transformation matrix

Takes in the rotation matrix and translation vector as input and stores the resultant homogeneous transformation matrix in H.

Parameters:
[in]PA vector containing the translation co-ordinates of the transform. Expressed as a 3x1 position vector
[in]Rot3x3 Rotation matrix
[out]HResultant transformation matrix obtained as H = [R P; 0 0 0 1]
Returns:
Returns 0 for no error.

Referenced by calc_collar_angles(), calc_joint_angle_err(), DH_hayati(), DHtoFix(), and gsl_pt_quat_matrix().

Here is the caller graph for this function:

void gsl_low_pass_filter_data ( double  data[],
int  num_frames,
int  num_freq 
)

References gsl_get_fourier_coeffs(), and gsl_low_pass_fourier().

Referenced by filter_accl(), filter_sync_passive_active(), main(), and opti_first_extrema().

Here is the call graph for this function:

Here is the caller graph for this function:

void gsl_low_pass_fourier ( double  data[],
gsl_vector *  f_data,
int  num_freq 
)

References gsl_vector2double().

Referenced by gsl_low_pass_filter_data().

Here is the call graph for this function:

Here is the caller graph for this function:

int gsl_pt_quat_matrix ( vector< double > &  Pvals,
vector< double > &  qvals,
gsl_matrix *  MatOP 
)

References gsl_hom_matrix(), and gsl_rodrigues_to_rot().

Referenced by calc_transf_matrix().

Here is the call graph for this function:

Here is the caller graph for this function:

void gsl_rodrigues_to_rot ( gsl_vector *  rodrig,
gsl_matrix *  Rot 
)

Referenced by calc_collar_angles(), and gsl_pt_quat_matrix().

Here is the caller graph for this function:

void gsl_Rot_matrix2vector ( gsl_matrix *  Rot,
gsl_vector *  v 
)

References gsl_vector_norme().

Here is the call graph for this function:

void gsl_rot_to_rodrigues ( gsl_matrix *  Rot,
gsl_vector *  rodrig 
)

Referenced by calc_collar_angles().

Here is the caller graph for this function:

void gsl_vector2Rot_matrix ( gsl_matrix *  Rot,
gsl_vector *  v 
)

References gsl_vector_norme().

Referenced by encaps_vector2rot().

Here is the call graph for this function:

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_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 ( 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 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:

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

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

Referenced by h_tib_low_theta_encaps().

Here is the caller graph for this function:

int h_tib_low_theta_encaps ( gsl_vector *  h,
gsl_vector *  state_vec 
)

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

References h_measurement_tib_low_theta().

Referenced by run_kalman_tib_low_theta().

Here is the call graph for this function:

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:

int jnt_angle_jacob_wrap ( const gsl_vector *  angle_rad,
struct angle_data *  angle_d,
gsl_matrix *  J 
)

References angle_data::DH_mat, jnt_angle_jacobian(), angle_data::Mat_loc, angle_data::Ptglob, and angle_data::Ptloc.

Referenced by calc_joint_ang().

Here is the call graph for this function:

Here is the caller graph for this function:

int jnt_angle_jacobian ( double  theta,
double  DH_mat[],
double  Mat_loc[],
double  Ptloc[3],
double  J[] 
)

Referenced by calc_jnt_angle_jacob_wrap(), and jnt_angle_jacob_wrap().

Here is the caller graph for this function:

int load_accl_data ( char *  filename,
int  num_accls,
int  num_accl_entries,
vector< vector< double > > &  datapoints 
)

Load Accelerometer Data for Postprocessing.

This function reads the data stored by accelerometers in files and loads it into vectors containing angular acceleration, angular velocity, magnetic forces, and quaternion representation of orientation.

Parameters:
[in]filenameName of the file where readings have been stored.
[in]num_acclsNumber of accelerometer readings in the file.
[in]num_accl_entriesNumber of entries per accelerometer reading in the file. Generally 13 entries per reading - accls(3), velocity(3), magnetic field(3), orientation in quaternions(4) and timestamp
[out]datapointsThe 2d vector containing all measurement data. First column contains the timestamps.

References accl_mario_orient(), and free_acc().

Referenced by load_collar_accl_data(), and main().

Here is the call graph for this function:

Here is the caller graph for this function:

int load_active_data ( char *  filename,
int  num_act,
vector< vector< double > > &  datapoints 
)

Load Active wire Data for Postprocessing.

This function reads the data stored by MARIONET system in files and loads it into vectors containing timestamp of reading and the wire length at that time.

Parameters:
[in]filenameName of the file where readings have been stored.
[in]num_actNumber of active wire readings in the file.
[out]datapointsThe 2d vector containing all measurement data. First column contains the timestamps.

This function also downsamples the data to 100Hz

Referenced by load_all_measures().

Here is the caller graph for this function:

int load_all_measures ( tibia &  tib,
femur &  fem,
vector< vector< double > > &  frame_refs 
)

Load all sensor data into leg structure.

load_all_measures function loads all sensor data recorded during experiments into the leg structures: tibia and femur. This function calls other postprocess functions that load respective sensor datas into the structures.

Parameters:
[in,out]tibTibia structure that contains tibial collar data and that will hold data for sensors fixed on tibia.
[in,out]femFemur structure containing femur collar data and that will hold data for sensors fixed on femur.
[in]Framereference vector (in MARIONET fixed reference system) The function will ask for the filenames containing sensor data from the user. The tib and fem structures already contain all config data.
Todo:
Adjust loading data for correct timestamp alignment.. or write a separate function to achieve that

vector containing coordinates of 3 frame reference points in optical frame of ref.

References tibia::accelerometers, tibia::accl_count, tibia::act_wire_count, calc_mario_frame_opti(), clean_opti_data(), convert_opti_to_mario(), filter_sync_passive_active(), load_active_data(), load_collar_accl_data(), load_collar_active_data(), load_collar_opti_data(), load_collar_passive_data(), load_mario_frame_opti_data(), load_opti_data(), load_passive_data(), sensor::mdata, tibia::opti_count, tibia::pass_wire_count, and sync_opti_active().

Here is the call graph for this function:

int load_collar_accl_data ( char *  filename,
tibia &  tib,
femur &  fem 
)

Load Accelerometer data for all Accelerometers on leg into tibia structure.

The load_accls_data functions pulls in motion data for each acclerometers into a 2d vector (each). This function further processes the data and assigns it to correct child structure (accelerometers) of the tibia structure. This is done by comparing the sensor id of the accelerometer stored in input vector with the of the in tib.accelerometers[i].label_id

Ensure that load_accl_data function has run before!

References tibia::accelerometers, tibia::accl_count, filter_accl(), sensor::label_id, load_accl_data(), and sensor::mdata.

Referenced by load_all_measures(), and main().

Here is the call graph for this function:

Here is the caller graph for this function:

int load_collar_active_data ( vector< vector< double > >  all_act_data,
tibia &  tib,
femur &  fem 
)

Load Active wire data for all active wires on leg into tibia and femur structure.

The load_collar_active_data functions pulls in motion data for each wire sensor into a 2d vector (each). This function further processes the data and assigns it to correct child structure (active_wires) of the tibia (and femur) structure. This is done by comparing the sensor id of the active wire stored in input vector with the of the in tib.active_wires[i].label_id

References tibia::act_wire_count, tibia::active_wires, and sensor::mdata.

Referenced by load_all_measures().

Here is the caller graph for this function:

int load_collar_calibs ( char *  foldername,
vector< vector< double > > &  Hprev,
vector< vector< double > > &  Pvals,
vector< vector< double > > &  qvals,
vector< double > &  firstlink,
vector< vector< double > > &  Plate 
)

Load Calibrated Collar Data: This function loads the information regarding calibrated collars into vectors for use in postprocessing.

The function takes in the name of the folder containing all the calibration information ie DH matrices, local plate based co-ordinate system information, obtained from the calculations done according to the methods described in the IFToMM paper.

The calibration files are all assumed to be stored under "/user/mharshe/home/work/knee-joint/collar-calib/". Separate sub-folders exist for tibia-upper, tibia-lower, thigh-upper, thigh-lower etc collars. Further sub-folders contain calibration data for specific LINK order (other than the default one). The program needs the name of this specific sub-folder corresponding to the actual LINK order as input. (The rest of the path is added automatically.)

Parameters:
[in]foldernameName of the folder containing calibration data.
[out]HprevVector containing DH (Hayati formulation) parameters
[out]PvalsCo-ordinate of the orgin of plates with respect to DH frame of the plate
[out]qvalsQuaternion describing orientation of plate-fixed local frame with respect to DH frame of the plate.
[out]firstlinkParameters for the first link.

The data read from the files is stored as vectors.

int load_collar_opti_data ( vector< vector< double > > &  all_opti_data,
vector< string > &  all_opti_labels,
vector< vector< int > >  point_flag,
tibia &  tib 
)

Load Optical Marker data for all optical sensors on collar into tibia structure.

The load_opti_data functions pulls in motion data for all markers into a 2d vector. Then, the clean_opti_data() cleans up the errors and mis-labeled tracks in the data and reassigns them to the correct tracks. This function further processes the data and assigns it to correct child structure (optical sensor) of the tibia structure. For best results, it is called after load_opti_data() and after clean_opti_data() have been run. This identification of data to sensor id is done by comparing the labels of the marker stored in input vector all_opti_labels with the labels of the markers in tib.optical[i].point_label

This function also converts the data to metric units ie converts from mm to m.

References sensor::mdata, tibia::opti_count, tibia::optical, sensor::point_flag, and sensor::point_label.

Referenced by load_all_measures().

Here is the caller graph for this function:

int load_collar_passive_data ( vector< vector< double > >  all_pass_data,
tibia &  tib,
femur &  fem 
)

Load Passive wire data for all passive wires on leg into tibia and femur structure.

The load_collar_passive_data functions pulls in motion data for each wire sensor into a 2d vector (each). This function further processes the data and assigns it to correct child structure (passive_wires) of the tibia (and femur) structure. This is done by comparing the sensor id of the passive wire stored in input vector with the of the in tib.passive_wires[i].label_id

References sensor::label_id, sensor::mdata, tibia::pass_wire_count, and tibia::passive_wires.

Referenced by load_all_measures().

Here is the caller graph for this function:

int load_mario_frame_opti_data ( vector< vector< double > > &  all_opti_data,
vector< string > &  all_opti_labels,
vector< vector< double > > &  frame 
)

Load the frame reference point coordinates.

Referenced by load_all_measures().

Here is the caller graph for this function:

int load_opti_data ( char *  filename,
vector< vector< double > > &  datapoints,
vector< string > &  point_labels 
)

Load Optical Marker Data.

This program reads the Motion Capture Data written in *.c3d format files and loads it into vectors containing the point data and the point labels.

Parameters:
[in]filenameName of the file (*.c3d) containing the optical marker data
[out]datapoints2d vector containing point displacement data. datapoints[i][j] contains global position data of parameter j, in the i+1 frame. (index for vector starts from 0)
[out]point_labelsThe labels for the points on which markers are placed. point_label[i] corresponds to the label for data in vector datapoints[][3i] to datapoints[][3i+2]

This function performs the same steps as the c3dreader program (/user/home/mharshe/work/c3d_handling/c3dreader.cpp) but instead of storing the data into a txt file, it loads it into vectors (which are passed as arguments to the function).

total number of frames stored last_field - first_field+1

Referenced by load_all_measures(), and main().

Here is the caller graph for this function:

int load_passive_data ( char *  filename,
int  num_act,
vector< vector< double > > &  datapoints 
)

Load Passive wire Data for Postprocessing.

This function reads the data stored by passive wire measurement system in files and loads it into vectors containing timestamp of reading and the wire length at that time.

Parameters:
[in]filenameName of the file where readings have been stored.
[in]num_actNumber of passive wire readings in the file.
[out]datapointsThe 2d vector containing all measurement data. First column contains the timestamps. The timestamps in converted to UTC format by calling adj_labview_tmstamp() function before storing.

References adj_labview_tmstamp().

Referenced by load_all_measures(), and main().

Here is the call graph for this function:

Here is the caller graph for this function:

void mat_passage_3pts ( gsl_vector *  v1,
gsl_vector *  v2,
gsl_vector *  v3,
gsl_matrix *  T 
)

References gramSchmidt().

Here is the call graph for this function:

int opti_first_extrema ( vector< vector< double > > &  datapoints,
int  num_markers 
)

References extract_data_point(), first_local_extrema(), and gsl_low_pass_filter_data().

Referenced by main(), and sync_opti_active().

Here is the call graph for this function:

Here is the caller graph for this function:

void print_results_lm ( gsl_multifit_fdfsolver *  s,
size_t  n,
size_t  p,
int  status,
gsl_matrix *  covar 
)

Print Least Square result.

Display the result found using Least Squares

References ERR, and FIT.

Referenced by calc_joint_ang(), and get_betas().

Here is the caller graph for this function:

void quat_invert ( gsl_vector *  quat)

Referenced by main().

Here is the caller graph for this function:

void quat_multiply ( gsl_vector *  M1,
gsl_vector *  M2,
gsl_vector *  ans0 
)

Multiply quaternions.

Referenced by accl_mario_orient(), main(), and quat_rotate().

Here is the caller graph for this function:

void quat_rotate ( gsl_vector *  quat1,
gsl_vector *  vect,
gsl_vector *  ans1 
)

Rotate vector by quaternion.

References quat_multiply().

Referenced by free_acc().

Here is the call graph for this function:

Here is the caller graph for this function:

int read_all_wire_bases ( vector< vector< double > > &  wire_bases)
void rodrigues_to_quat ( gsl_vector *  quat_rod,
gsl_vector *  quat 
)

Convert quaterion in rodrigues formulation to normal quaternion.

void rotEuler ( double  X,
double  Y,
double  Z,
gsl_matrix *  Rot 
)

References rotX(), rotY(), and rotZ().

Here is the call graph for this function:

void rotX ( double  alpha_rad,
gsl_matrix *  Rot 
)

Rotation matrix about axis X, angle in radians.

Referenced by DH_hayati(), DHtoFix(), and rotEuler().

Here is the caller graph for this function:

void rotY ( double  alpha_rad,
gsl_matrix *  Rot 
)

Rotation matrix about axis Y, angle in radians.

Referenced by DH_hayati(), and rotEuler().

Here is the caller graph for this function:

void rotZ ( double  alpha_rad,
gsl_matrix *  Rot 
)

Rotation matrix about axis Z, angle in radians.

Referenced by calc_collar_angles(), calc_joint_angle_err(), DH_hayati(), DHtoFix(), and rotEuler().

Here is the caller graph for this function:

int run_kalman_femur ( femur &  fem,
vector< vector< double > >  wire_A_points,
gsl_vector *  tot_wire_len,
vector< vector< double > > &  state,
vector< double >  thetas 
)

Run the EKF on the femur.

Parameters:
[in]femThe femur structure with all data
[in]wire_A_pointsBase points of the active and passive wires connected to femur. (stored in the order as represented by the jacob equations) ie leftmost wire is labeled 1, irrespective of it being active or passive
[in]tot_wire_lenTotal wire lengths of the active wires, necessary for calculating measurement vector
[out]stateThe state vector, for each iteration. Output of the kalman filter

References tibia::accelerometers, tibia::active_wires, ekf_kalman_gain(), ekf_predict_step(), ekf_update_cov(), ekf_update_estimate(), generate_jacobian_femur(), generate_mes_vec_femur(), h_femur_encaps(), sensor::mdata, tibia::opti_count, tibia::optical, sensor::point_flag, set_initial_estimate(), set_proc_cov_mat(), and set_process_mat().

Here is the call graph for this function:

int run_kalman_femur_theta ( femur &  fem,
vector< vector< double > > &  state 
)

Run the EKF on the femur.

Parameters:
[in]femThe femur structure with all data
[out]stateThe state vector, for each iteration. Output of the kalman filter

References ekf_kalman_gain(), ekf_predict_step(), ekf_update_cov(), ekf_update_estimate(), generate_jacobian_femur_theta(), generate_mes_vec_femur(), h_femur_theta_encaps(), tibia::opti_count, and set_mes_err_cov().

Here is the call graph for this function:

int run_kalman_tib_low_theta ( tibia &  tib,
vector< vector< double > > &  state 
)

Run the EKF on the femur.

Parameters:
[in]femThe femur structure with all data
[out]stateThe state vector, for each iteration. Output of the kalman filter

References ekf_kalman_gain(), ekf_predict_step(), ekf_update_cov(), ekf_update_estimate(), generate_jacobian_tib_low_theta(), generate_mes_vec_tib_low(), h_tib_low_theta_encaps(), sensor::mdata, tibia::opti_count, tibia::optical, and set_mes_err_cov_tib().

Here is the call graph for this function:

int set_initial_estimate ( gsl_vector *  state_estimate)

Referenced by run_kalman_femur().

Here is the caller graph for this function:

int set_proc_cov_mat ( gsl_matrix *  Q)

Referenced by run_kalman_femur().

Here is the caller graph for this function:

int set_process_mat ( gsl_matrix *  A,
double  deltaT 
)

Referenced by run_kalman_femur().

Here is the caller graph for this function:

int set_theta_err_cov ( gsl_matrix *  Q,
int  num_theta 
)
int solve_gsl_lm ( gsl_multifit_function_fdf *  f,
gsl_vector_view  xv,
double  precision,
int  nbre_iter,
gsl_multifit_fdfsolver *  s,
gsl_matrix *  covar 
)

solve lm : rempli s avec la solution et covar avec la covariance a partir de la fonction f, du vecteur initial xv avec la methode de levenberg-marquardt pour une precision et un nombre d'iteration max

calcul de l'erreur

Referenced by calc_joint_ang(), and get_betas().

Here is the caller graph for this function:

int sync_opti_active ( vector< vector< double > > &  active_wire_data,
vector< vector< double > > &  optical_data,
int  num_opti_markers 
)

Sync optical data with active_wire system.

Active wire system has relatively low levels of measurement noise. Hence it is quite suitable for syncing optical data. This function would ideally perform best when used with readings of active wire 1,3 or 4. Function to be run before assigning optical data vector to tibia or femur structures

References first_local_extrema_vec(), IsOdd(), and opti_first_extrema().

Referenced by load_all_measures().

Here is the call graph for this function:

Here is the caller graph for this function:

 All Data Structures Files Functions Variables Typedefs Defines