Tuning the AttitudeEstimator in hrpsys state observation

Parameters

You can get parameters from the Python console of your Choreonoid simulation by

ae.getProperties()  # in our OpenRTM script ae is the AttituteEstimator

Covariances

The AttitudeEstimator has a number of covariance parameters describing how much the estimator "trusts" a given source. Low covariance means high trust, high covariance means low trust.

• state_init_cov: initial state covariance increase the value to decrease the confidence in the initial guess of the estimator (default: 1e-8)
• state_cov: current state covariance (default: 3e-14)
• gyr_cov: covariance on gyrometer measurements (default: 1e-10)
• acc_cov: covariance on accelerometer measurements (default: 0.003)
• lin_acc_cov: The covariance of the linear acceleration disturbance (default: 1e-13)
• ori_acc_cov: The covariance of the angular acceleration input to the system (default: 0.003)

Other parameters

• offset: legacy feature, allows to add an offset to the estimation (default: 0,0,0)
• sampling_time: time between two control cycles (e.g. 0.005 for HRP-4)

Example

In a first simulation on the HRP4Comanoid/sim_mc_udp.cnoid project, the estimator has a constant offset in the lateral direction. A quick check in the Python console revealed a first problem:

Python 2.7.6 (default, Nov 13 2018, 12:50:15) 
[GCC 4.8.4]
>>> ae.getProperties()
{'state_init_cov': '1e-08', 'state_cov': '3e-14', 'gyr_cov': '1e-10', 'lin_acc_cov': '1e-13', 'sampling_time': '0.002', 'ori_acc_cov': '0.003', 'compensateMode': '1', 'debug_level': '0', 'acc_cov': '0.003', 'offset': '0,0,0'}

The sampling time of HRP-4 is 0.005 rather than 0.002. This being fixed, the main cause for the constant offset was the covariance on gyro measurements which was too low (i.e. the estimator trusts them too much). We lowered it to 1e-6, also lowering state_init_cov to 1e-4:

>>> ae.setProperty("state_init_cov", "1e-4")
True
>>> ae.setProperty("gyr_cov", "1e-6")
True

Here is the plot used to debug this:

Convergence of the estimator happens after we set gyr_cov to its new value. Oscillations in the end are some walking motions.