ImuBase.calc_position wrongly uses normalized acc

[jonathanjfshaw]

I understand little about sensor fusion. But I made the following test script:

from skinematics import imus, quat, vector, rotmat
from skinematics.simulations.simulate_movements import simulate_imu
import numpy as np
from skinematics.sensors.manual import MyOwnSensor

# Simulate IMU-data
duration_movement = 1  # [sec]
duration_total = 1 # [sec]
rate = 100  # [Hz]
direction = [1, 0.5, 0.25]
distance = 1
imu, body = simulate_imu(rate, duration_movement, duration_total, direction=direction, distance=distance)

in_data = {'rate': rate,
        'acc': imu['gia'],
        'omega':imu['omega'],
        'mag':imu['magnetic']
           }
np.set_printoptions(precision=3, suppress=True)
print("actual pos")
print(body['pos'][-1])
print("actual quat")
print(body['quat'][-1])

def test(q_type):
    my_sensor = MyOwnSensor(in_data=in_data, q_type=q_type, calculate_position=True)
    print(q_type + " pos")
    print(my_sensor.pos[-1])
    print(q_type + " quat")
    print(my_sensor.quat[-1])

for q_type in ['analytical', 'kalman', 'madgwick', 'mahony']:
    test(q_type)

I get these results:

actual pos
[0.873 0.436 0.218]
actual quat
[1. 0. 0. 0.]
analytical pos
[0.873 0.436 0.218]
analytical quat
[1. 0. 0. 0.]
kalman pos
[ 0.039  0.029 -4.414]
kalman quat
[ 1.    -0.013  0.013  0.015]
Calculating the Quaternions [#########################] 100/100
madgwick pos
[-0.059  0.014 -4.422]
madgwick quat
[ 0.965  0.004 -0.259  0.034]
Calculating the Quaternions [#########################] 100/100
mahony pos
[-0.011  0.015 -4.42 ]
mahony quat
[ 0.989 -0.004 -0.15   0.001]

All the methods apart from analytical have the position completely wrong despite getting the quaternion mostly right.

If I change the duration so that there's a stationary period after the movement then the results seem like a disaster. Even the analytical method starts making a mess of the position and the other methods are hopelessly wrong even about the quaternion.

[jonathanjfshaw]

I've identified the bug. As part of the quaternion calculations self.acc gets vector normalized. But when calculating position, we need to use the original un-normalized acc; this is no longer stored on self.acc because of python's passing by reference.

If you modify my script above like this:

def test(q_type):
    my_sensor = MyOwnSensor(in_data=copy.deepcopy(in_data), q_type=q_type, 
 calculate_position=True)
   # reset the acc as it has now been overwritten by vector normalized acc
    my_sensor.acc = imu['gia']
    # Use the true quat not the estimated one, to isolate errors to the position calculations
    my_sensor.quat = body['quat']
    # redo the position calculations with the proper acc and true quat
    my_sensor.calc_position()
    print(q_type + " pos")
    print(my_sensor.pos[-1])

Then you get:

actual pos
[0.873 0.436 0.218]
analytical pos
[0.873 0.436 0.218]
kalman pos
[0.873 0.436 0.218]
madgwick pos
[0.873 0.436 0.218]
mahony pos
[0.873 0.436 0.218]

These are the lines in imus.py that are corrupting the values of self.acc for madgwick and mahony:

            Acc = vector.normalize(self.acc)
            Mag = vector.normalize(self.mag)

Why kalman is also affected, I don't know.