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"
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. |
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 PI 3.14159 |
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.
[in] | quat_mario | Quaternion describing orientation of MARIONET w.r.t earth fixed system |
[in,out] | quat | Quaternion 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().
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.
[in,out] | tmpstamp | Number of seconds elapsed since epoch |
Referenced by load_passive_data().
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().
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().
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.
[in] | tib | The tibia structure holding all info |
[in] | linkname | The name of the link that is to be treated as the base link |
[in] | fr_num | The frame number for which the measured data is taken for calculations |
[out] | M | The 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.
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
[in,out] | H | 4x4 matrix that transform coordinates in optical frame to marionet frame |
[in] | frame_mario | 2d vector containing the 3 coordinates of ref points in mario frame |
[in] | frame | 2d vector containing the 3 coordinates of ref points in optical frame |
References calc_trans_mat(), and vectormat2double().
Referenced by load_all_measures().
int calc_trans_mat | ( | double | Ploc[], |
double | Pglob[], | ||
gsl_matrix * | H | ||
) |
Given coordinates of 3 points in 2 frames, calculate transformation matrix relating them.
[in] | Ploc | 9x1 array with coordinates of 3 points in local frame, concatanated together. |
[in] | Pglob | 9x1 array with coordinates of 3 points in global frame, concatanated together. |
[out] | H | 4x4 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().
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().
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().
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!
[in,out] | datapoints | Vector containing point data. |
[out] | point_flag | Vector containing flag = 1 if point is reliable, 0 if not. Same size as datapoints vector |
[in] | point_labels | Labels 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().
int convert_opti_to_mario | ( | tibia & | tib, |
gsl_matrix * | H | ||
) |
Convert optical marker data in tibia structure to MARIONET reference based coordinate system.
[in,out] | tib | Tibia (or femur) structure containing all optical data points |
[in] | H | Transformation matrix to convert coordinates in optical frame to MARIONET frame |
References sensor::mdata, tibia::opti_count, and tibia::optical.
Referenced by load_all_measures().
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().
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().
void encaps_vector2rot | ( | double * | vrot, |
double * | Rot | ||
) |
References gsl_matrix2double(), and gsl_vector2Rot_matrix().
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().
int filter_accl | ( | vector< vector< double > > & | accl_data | ) |
References extract_data_point(), and gsl_low_pass_filter_data().
Referenced by load_collar_accl_data().
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)
[in] | active_wire_data | Active wire data needed to synchronize passive data |
[in,out] | passive_wire_data | Passive wire data to remove spurious noise. |
References extract_data_point(), and gsl_low_pass_filter_data().
Referenced by load_all_measures().
int first_local_extrema | ( | double * | data, |
int | num_iter, | ||
int | frame_start | ||
) |
Find the first local extrema in the vector of doubles.
[in] | data | Array containing noise free version of data |
Referenced by opti_first_extrema().
int first_local_extrema_vec | ( | vector< vector< double > > | data, |
int | index, | ||
int | index_step | ||
) |
Find the first local extrema in the vector of doubles.
[in] | data | Vector containing noise free version of data, along with timestamps in column 0 of the vector |
[in] | index | The index of the column to be used to find extrema |
[in] | index_step |
Referenced by sync_opti_active().
void free_acc | ( | double * | accl, |
double * | quat | ||
) |
Subtract gravitational acceleration from accl readings.
[in,out] | accl | Acceleration vector. The gravity-free component is stored back |
[in] | quat | Quaternion describing orientation of acclerometer w.r.t earth fixed system |
References quat_rotate().
Referenced by load_accl_data().
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
[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 |
References gsl_vector2double(), jacob_set_tot(), and vectormat2double().
Referenced by run_kalman_femur().
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
[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 |
Referenced by run_kalman_femur_theta().
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
[out] | J | 14x4 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 | 4x1 state vector containing [theta1,..., theta4] |
[in] | fem | Femur structure containing all config data and measurements |
Referenced by run_kalman_tib_low_theta().
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.
[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 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.
[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
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().
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.
[out] | mes_vector | The 14x1 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.
Referenced by run_kalman_tib_low_theta().
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().
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().
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.
References getdist().
Referenced by clean_opti_data().
double getdist | ( | vector< double > & | basepoint, |
vector< double > & | newpoint | ||
) |
Get distance between basepoint and newpoint.
Referenced by clean_opti_data(), and getcang().
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().
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().
void gsl_filter_data | ( | double | data[], |
int | num_frames, | ||
int | num_dominant_freq | ||
) |
References get_fourier_dominant_estimate(), and gsl_get_fourier_coeffs().
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.
[in] | data[] | 1d array containing the data to be filtered. |
Referenced by gsl_filter_data(), and gsl_low_pass_filter_data().
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.
[in] | P | A vector containing the translation co-ordinates of the transform. Expressed as a 3x1 position vector |
[in] | Rot | 3x3 Rotation matrix |
[out] | H | Resultant transformation matrix obtained as H = [R P; 0 0 0 1] |
Referenced by calc_collar_angles(), calc_joint_angle_err(), DH_hayati(), DHtoFix(), and gsl_pt_quat_matrix().
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().
void gsl_low_pass_fourier | ( | double | data[], |
gsl_vector * | f_data, | ||
int | num_freq | ||
) |
References gsl_vector2double().
Referenced by gsl_low_pass_filter_data().
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().
void gsl_rodrigues_to_rot | ( | gsl_vector * | rodrig, |
gsl_matrix * | Rot | ||
) |
Referenced by calc_collar_angles(), and gsl_pt_quat_matrix().
void gsl_Rot_matrix2vector | ( | gsl_matrix * | Rot, |
gsl_vector * | v | ||
) |
void gsl_rot_to_rodrigues | ( | gsl_matrix * | Rot, |
gsl_vector * | rodrig | ||
) |
void gsl_vector2Rot_matrix | ( | gsl_matrix * | Rot, |
gsl_vector * | v | ||
) |
References gsl_vector_norme().
Referenced by encaps_vector2rot().
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().
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().
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().
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().
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().
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().
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().
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().
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().
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.
[in] | filename | Name of the file where readings have been stored. |
[in] | num_accls | Number of accelerometer readings in the file. |
[in] | num_accl_entries | Number 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] | datapoints | The 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().
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.
[in] | filename | Name of the file where readings have been stored. |
[in] | num_act | Number of active wire readings in the file. |
[out] | datapoints | The 2d vector containing all measurement data. First column contains the timestamps. |
This function also downsamples the data to 100Hz
Referenced by load_all_measures().
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.
[in,out] | tib | Tibia structure that contains tibial collar data and that will hold data for sensors fixed on tibia. |
[in,out] | fem | Femur structure containing femur collar data and that will hold data for sensors fixed on femur. |
[in] | Frame | reference 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. |
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().
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().
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().
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.)
[in] | foldername | Name of the folder containing calibration data. |
[out] | Hprev | Vector containing DH (Hayati formulation) parameters |
[out] | Pvals | Co-ordinate of the orgin of plates with respect to DH frame of the plate |
[out] | qvals | Quaternion describing orientation of plate-fixed local frame with respect to DH frame of the plate. |
[out] | firstlink | Parameters 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().
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().
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().
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.
[in] | filename | Name of the file (*.c3d) containing the optical marker data |
[out] | datapoints | 2d 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_labels | The 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().
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.
[in] | filename | Name of the file where readings have been stored. |
[in] | num_act | Number of passive wire readings in the file. |
[out] | datapoints | The 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().
void mat_passage_3pts | ( | gsl_vector * | v1, |
gsl_vector * | v2, | ||
gsl_vector * | v3, | ||
gsl_matrix * | T | ||
) |
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().
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
Referenced by calc_joint_ang(), and get_betas().
void quat_invert | ( | gsl_vector * | quat | ) |
void quat_multiply | ( | gsl_vector * | M1, |
gsl_vector * | M2, | ||
gsl_vector * | ans0 | ||
) |
Multiply quaternions.
Referenced by accl_mario_orient(), main(), and quat_rotate().
void quat_rotate | ( | gsl_vector * | quat1, |
gsl_vector * | vect, | ||
gsl_vector * | ans1 | ||
) |
Rotate vector by quaternion.
References quat_multiply().
Referenced by free_acc().
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 | ||
) |
void rotX | ( | double | alpha_rad, |
gsl_matrix * | Rot | ||
) |
Rotation matrix about axis X, angle in radians.
Referenced by DH_hayati(), DHtoFix(), and rotEuler().
void rotY | ( | double | alpha_rad, |
gsl_matrix * | Rot | ||
) |
Rotation matrix about axis Y, angle in radians.
Referenced by DH_hayati(), and rotEuler().
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().
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.
[in] | fem | The femur structure with all data |
[in] | wire_A_points | Base 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_len | Total wire lengths of the active wires, necessary for calculating measurement vector |
[out] | state | The 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().
int run_kalman_femur_theta | ( | femur & | fem, |
vector< vector< double > > & | state | ||
) |
Run the EKF on the femur.
[in] | fem | The femur structure with all data |
[out] | state | The 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().
int run_kalman_tib_low_theta | ( | tibia & | tib, |
vector< vector< double > > & | state | ||
) |
Run the EKF on the femur.
[in] | fem | The femur structure with all data |
[out] | state | The 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().
int set_initial_estimate | ( | gsl_vector * | state_estimate | ) |
int set_proc_cov_mat | ( | gsl_matrix * | Q | ) |
int set_process_mat | ( | gsl_matrix * | A, |
double | deltaT | ||
) |
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().
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().