Matlab and C++ code for implementation of the Extended Kalman Filter for estimating dynamic quantities for a single rigid body with distributed force/torque measurements and distributed gyroscopes and accelerometers measurements.It also include estimation of the orientation under the quaternion representation.
@prashanthr05 For a sanity check on the results, it is recommended that the following checklist is followed:
Check IMU rotation
Check CoM frame position and orientation
Check sensor homogenous transforms in CoM frame (com_X_imu, com_X_ft)
verify raw data signs
sanity check on raw data magnitudes
verify calibrated and rotated data signs
sanity check on calibrated and rotated data magnitudes
verify process model equations
verify measurement model equations
Tuning must be performed only after each of these are verified through checking the measured data. Both the calibration phase (robot standing) and dynamic phase (robot toppling) parts must be verified.
Exact relationship between ankle F/T readings and skin in the normalForce version as done in load_n_filter.m. answer: What has to be multiplied by the stiffness vector is just the raw third component of the force measurement as seen in line 38 of get_input_output.m. Should we use the norm instead?
Remove inactive taxels from Xts and yts
Recompute the stiffness vector based on skin normal forces only.
Solve the rank defficiency induced by subsampling of the training dataset. answer Temporarily solved by choosing a subsample for which the submatrix is full column rank(Xtr).
Save idxTaxelsNeverActive from load_n_filter() in test_on_large_data() to remove the same taxels in our collected skin data, as in this estimation the always inactive taxels have been removed. Solution The function for estimation must be called in the following way [best_w, best_w_lsqlin, activeTaxelsIndeces] = test_on_large_data() and activeTaxelsIndeces which corresponds to a vector of all the taxels that were activated at least once in the dataset MUST be stored an added to /localParamEKF/skinFunctions.
Fix function plot_weights_on_skin() to plot foot with the new reduced weights vector containing taxels whose stiffnes is not estimated (zero is being assigned to those taxels). Solution: A new vector best_w_padded was created containing the weights (stiffness values) of the taxels for which the estimation was performed, and zero for all the others.
Change plot_weights_on_skin() such that the foot drawing does not contain those awful black lines of the mesh.
Update code to compute total normal force through the skin using the latest stiffness vector.
Add function axisAngle2rotationMatrix
Obtain roto-translation from upperleg (thigh sensor) to WRF and ankle sensor to WRF for the initial leg configuration we used in the experiments. Solution: in /localParamEKF/utils you will find a simulink model called mdlForRotoTransMatrices which uses iCubHeidelberg01 and retrieves the desired rototranslations saving them in .mat files.
Choose the two experiments and filter starting and ending points of the dataset.
General Pending Tasks
Recomputation of all measurements in FOOT COM REF FRAME
Extract new Tmatrix from CAD files for right foot.
Integration of skin data in realMeasurent.m separating right and left foot.
Skin stiffness covariance estimate through Bayesian Network.
Use stiffness matrix which can be found in fb1e74e to compute (total?) external force applied on the foot sole and COP.
Review orientation estimation.
Incorporate IMU with connection to head. Consider mounting position on foot in order to have repeatable experiments
Redo simulation to have longer durations without hitting singularities.
Experiment with torque inputs which maintain an orientation trajectory within singularity limits
Data collection 18-02-2015
Prepare experimental setups.
IMU rototranslation matrix tuning.
Tipping scenario.
Orientation perturbation.
Prepare scripts for retrieving experimental data [dataDumper, macumba, ecc]
Record spare video to replicate experiments next week (FROM RED ROBOT SINCE BLACKIE IS ON HOLIDAYS)
The CSS was not being rendered online due the wrong value for url in _config.yml which should be set to url: /extended-kalman-filter prior to the commit of your website. If on the other hand it is being deployed on localhost:4000 the previous line should be commented or set to "".
Remember also that to deploy your site locally you must run: bundle exec jekyll serve --watch --baseurl ''
This cmake module should be able to find XSens installed headers and libraries for the correct architecture. A first version working on Linux would be highly appreciated.
I installed the EKF library and tried to run the script testEKF_completeLegWithSkin.m under the EKF_EulerAngles folder of dynamic-tests branch. I can see the plots being plotted but it suspends showing the following error,
"Attempt to reference field of non-structure array.
Error in plotResultsOutput_withSkin (line 19)
figPreN = figPreN.Number - 1;
Error in plotAndSaveFigs (line 2)
plotResultsOutput_withSkin(dataBaseFolder);
Error in testEKF_completeLegWithSkin (line 227)
plotAndSaveFigs(dataBaseFolder,plotFigBaseFolder);"
The module as of today is fully written and should be "functional". This doesn't mean that the estimate is correct. This issue has been created simply to analyze in an organized way the filtering process in this module. Up next, the different stages by which it is composed
Correct initialization
All the filter parameters are passed and set correctly to their initial values in the corresponding configuration file.
Initial gaussian system noise uncertainty (mean and covariance) makes sense.
Initial gaussian measurement noise uncertainty (mean and covariance) makes sense.
Priors make sense (mu and covariance)
Online Estimation
Convert read angular velocity to rad/s
nonLinearAnalyticConditionalGaussian::ExpectedValueGet() should return the state prior prediction.
nonLinearAnalyticConditionalGaussian::dfGet() should return the transition matrix.
nonLinearMeasurementGaussianPdf::ExpectedValueGet()` should return the acceleration model output.
nonLinearMeasurementGaussianPdf::dfGet() should return the jacobian evaluated at the current estimate.
If the IMU is recognized via USB, you should read this after lsusb Bus 002 Device 006: ID 0403:d38b Future Technology Devices International, Ltd
In the directory MT_Software_Suite_Linux_4.3 you want to run mtsdk_linux_4.3.sh which is the MT SDK.
If you run into this error message
'realpath' could not be found. It is usually installed with the 'realpath' package
'uudecode' could not be found. It is usually installed with the 'sharutils' package
If you want to install in the default directory /usr/local just hit Enter when it asks to Enter the installation directory. Afterwards you will find in this directory all the libraries, headers and examples necessary for you to start collecting data straight from the sensor.
The first example you wanna try is in /usr/local/xsens/examples/mtsdk/src_cpp. To compile do: make src_cpp. Enter src_cpp cd src_cpp and run ./example
If it recognizes the device but it can't access the device do the following as found in README.MTSDK
The device is recognized, but I cannot ever access the device -
Make sure you are in the correct group (often dialout or uucp) in order to
access the device. You can test this with
$ ls -l /dev/ttyUSB0
crw-rw---- 1 root dialout 188, 0 May 6 16:21 /dev/ttyUSB0
$ groups
dialout audio video usb users plugdev
If you aren't in the correct group, you can fix this in two ways. 1. Add yourself to the correct group You can add yourself to it by using your distributions user management tool, or call $ sudo usermod -G dialout -a $USER Be sure to replace dialout with the actual group name if it is different. After adding yourself to the group, either relogin to your user, or call $ newgrp dialout to add the current terminal session to the group.
It is strongly recommended to log out and back in after these changes for them to take effect
The output of the example module will look like this: W:-0.72,X: 0.22,Y:-93.12,Z: 0.00,Roll: -82.13,Pitch: 90.00,Yaw: -89.80^C0.00,Yaw: -90.000w: 90.00: -90.000