Sensor fusion reference frame
I'm trying to get a 2 sensor solution working with sensor fusion. After successfully calibrating them, I have expected them to output quaternions in the same reference frame. (I.e. give the same quaternions when they are oriented the same way.) This was not the case. Please see these images:
The sensor pairs on the images are oriented so that they output the same quaternions, with around 0.001 accuracy on each of the quaternion's components. This is obviously not good, as they have visibly different orientations.
The table is made from wood. The closest metal was a laptop, around 25 cm away.
Sensor model: MetaMotion R
Model number: 5
Hardware: 0.4, 0.3
C++ SDK: 0.18.4
iOS SDK: 3.3.0 (I'm aware it isn't released to Cocoapods.org yet.)
Can you please advise on how to debug this issue?
In case this is related to sensor calibration, please find below the sequence of SDK calls that we're doing to get the mismatching quaternions.
Pairing (first sensor, then second)
1. Bluetooth scan until an as-yet unpaired sensor is found with
rssi > -75. Once found, stop the scan.
MetaWear.clearAndReset(), then wait for the sensor to disconnect.
mbl_mw_sensor_fusion_stop() (just in case, even though sensor fusion probably wasn't running.)
MetaWear.flashLED(color: ..., intensity: 1, _repeat: 5, onTime: 500, period: 1500)
1. Bluetooth scan for paired sensors. (It always found the 2 sensors in the test.)
MetaWear.connectAndSetup() (first sensor, then second. Skipped if
mbl_mw_sensor_fusion_calibration_state_data_signal(board) to retrieve the calibration state signal.
5. Record a
calib_start macro with the following operations if it wasn't recorded. If it's recorded already, do nothing:
mbl_mw_sensor_fusion_set_mode(board, MBL_MW_SENSOR_FUSION_MODE_NDOF) mbl_mw_sensor_fusion_set_acc_range(board, MBL_MW_SENSOR_FUSION_ACC_RANGE_16G) mbl_mw_sensor_fusion_set_gyro_range(board, MBL_MW_SENSOR_FUSION_GYRO_RANGE_2000DPS) mbl_mw_sensor_fusion_write_config(board) mbl_mw_sensor_fusion_start(board) var pattern = MblMwLedPattern() mbl_mw_led_load_preset_pattern(&pattern, MBL_MW_LED_PRESET_SOLID) mbl_mw_led_write_pattern(board, &pattern, MBL_MW_LED_COLOR_RED) mbl_mw_led_play(board)
6. Record a
calib_end macro with the following operation if it wasn't recorded. If it's recorded already, do nothing:
mbl_mw_datasignal_subscribe(signal, contextPtr, receivedData) with the calibration state signal. (both sensors)
8. Execute the
calib_start macro. (both sensors)
mbl_mw_datasignal_read(signal) around every 500 milliseconds with the calibration state signal. (both sensors)
10. Wait until all 3 fields in the periodically received calibration state are
SENSOR_FUSION_CALIBRATION_ACCURACY_HIGH for a sensor.
mbl_mw_datasignal_unsubscribe(signal) with the calibration state signal. (for the sensor that's done)
13. Execute for the sensor that's done:
mbl_mw_led_stop_and_clear(board) var pattern = MblMwLedPattern() mbl_mw_led_load_preset_pattern(&pattern, MBL_MW_LED_PRESET_SOLID) mbl_mw_led_write_pattern(board, &pattern, MBL_MW_LED_COLOR_GREEN) mbl_mw_led_play(board)
mbl_mw_sensor_fusion_read_calibration_data(...) to receive the calibration data, then wait for it to get the result.
15. Execute the
calib_stop macro. (for the sensor that's done)
16. Wait for all sensors to be done, then
mbl_mw_led_stop_and_clear(board) mbl_mw_sensor_fusion_write_calibration_data(board, &calibration)
I'm aware that the recommended way to store the calibration data is to create an on-boot macro with it. I intend to implement it, once I can see the system functional and working. For now, I guess this should work.
1. For the first sensor, then the second:
mbl_mw_led_stop_and_clear(board) mbl_mw_sensor_fusion_clear_enabled_mask(board) mbl_mw_sensor_fusion_enable_data(board, MBL_MW_SENSOR_FUSION_DATA_QUATERNION) qSignal = mbl_mw_sensor_fusion_get_data_signal(board, MBL_MW_SENSOR_FUSION_DATA_QUATERNION)
2. Then I set up the following data processor chain for both sensors:
qSignal -> Sample(binSize: 0) -> Time(mode: absolute, period: 25ms) -> Passthrough(mode: conditional, count: 0) -> Log
I'm fully aware that it doesn't make much sense for this test. The
binSize parameter of the Sample data processor and the Passthrough's
count is adjusted for other implemented use cases. I described the full data processor chain for the sake of completeness. There is no other on-sensor programming done. (I expect the
binSize=0 to just let the data through without delaying it.)
mbl_mw_sensor_fusion_start(board) for both sensors
mbl_mw_datasignal_subscribe(signal, contextPtr, receivedData) for the
Time(mode: absolute, period: 25ms) signal created above.
data.valueAs() as MblMwQuaternion values to the console for both sensors as soon as they're received.
Please note that all sensor-related code runs on
MetaWear.apiAccessQueue, which is a serial queue, so even operations that I've noted as parallel above really run sequentially. I don't think that this is a multithreading-related issue. I also verified that the order of operations is in fact as I noted above.