Novice: Data to use for EKF with sensor

[rhyshorner]

Hello,
I'd like to start by saying this repo is incredible, there are so many types and styles of orientation estimation, it must've been a massive project to undertake. Amazing.

I am going to use it to build a robot car as a personal learning project and an attempt to push my skills up a level.

So the question;
I am trying to use your EKF module to get some orientation information, I am using the sensor ICM20948 which has an accelerometer, gyro and magnetometer.

To start the script, imports and sensor initialise;

from icm20948 import ICM20948
from ahrs.filters import EKF
from ahrs.common.orientation import acc2q
import numpy as np

imu = ICM20948()

The information I get from the ICM20948 is 9 individual float type variables, in my code it looks like;

x, y, z = imu.read_magnetometer_data()
ax, ay, az, gx, gy, gz = imu.read_accelerometer_gyro_data()

which I then convert into numpy arrays for the module;

mag_data = np.array([x, y, z])
acc_data = np.array([ax, ay, az])
gyr_data = np.array([gx, gy, gz])

then I create the ekf object

ekf = EKF(gyr=gyr_data, acc=acc_data)

and after this I have the mainloop, the Q part confuses me because it seem like there should already be the quaternion information before it makes the estimation.

while True:
    x, y, z = imu.read_magnetometer_data()
    ax, ay, az, gx, gy, gz = imu.read_accelerometer_gyro_data()
    mag_data = np.array([x, y, z])
    acc_data = np.array([ax, ay, az])
    gyr_data = np.array([gx, gy, gz])

    Q = ekf.update(Q, gyr_data[t], acc_data[t])
    print(Q)

Basically I'm trying to integrate the ICM20948 with your EKF filter module, I am quite novice and in-experienced, so please forgive any very obvious mistakes.

Also on a completely separate topic, when I try to import the Tilt package it doesnt seem to be able to find it. whereas the EKF package imports just fine.
Python3.7.3

>>> from ahrs.filters import Tilt
Traceback (most recent call last):
  File "<stdin>", line 1, in <module>
ImportError: cannot import name 'Tilt' from 'ahrs.filters' (/home/pi/.local/lib/python3.7/site-packages/ahrs/filters/__init__.py)
>>> from ahrs.filters import EKF
>>> 

thanks,
Rhys.

[Mayitzin]

Hi Rhys,

I'm glad the package is useful for your projects.

First, just to be clear, if you give the sensor data at the construction of the EKF object, it will try to estimate all orientations automatically and store it in the attribute Q, which obviously wouldn't work if you are giving only one sample per sensor and no previous quaternion. Let's remember that Kalman Filters always require the definition of the previous state (k-1) in order to estimate the current state (k).

If you want to avoid this, then build the object without giving any sensor data:

ekf = EKF()

In this case the object is created, but no estimations are done yet, and the attribute Q is not created. This way you can estimate the orientations later in your own loop.

Regarding the Q part of the estimation: it is an N-by-4 array with the computed estimation of ALL N quaternions and is defined IF all sensors measurements are given at the beginning, or if you manually create it. In your case, the estimation will be done in real-time, right? So, this big array is not gonna be created.

All implementations of the EKF require an initial state, in this case an initial orientation represented as a quaternion, so that the following estimations can be built on top of it:

q_0 -> q_1 -> ... q_n

You can do this by setting this initial orientation at the construction of the object:

ekf = EKF(q0=[1.0, 0.0, 0.0, 0.0])

In this example I set it to the identity quaternion ([1, 0, 0, 0]), but you can set it to any unit quaternion (a quaternion with norm equal to 1) representing the initial orientation.

A second detail to point out is that some sensors (especially low cost not used in aeronautics) tend to use a different reference frame called ENU (East-North-Up). It might be that yours could be using it too. You can also set this different frame at the construction of the object:

ekf = EKF(q0=[1.0, 0.0, 0.0, 0.0], frame='ENU')

Otherwise, it will be set to the default 'NED' (North-East-Down) used often in aeronautics.

For the case of the Tilt I cannot really reproduce the error. It seems to be working fine on my side. Which version are you using?

An alternative to that problem would be using the function acc2q, which returns a quaternion from a given tri-axial accelerometer sample that can be used as the initial quaternion:

q = ahrs.common.orientation.acc2q(acc_data)
ekf = EKF(q0=q)

Also, don't forget to set the sampling frequency. The default value is equal to 100 Hz, but if you are giving sensor signals at a different rate, you must set it in the constructor. For example, for 200 Hz:

ekf = EKF(frequency=200.0)

In summary, for your real-time application, a convenient solution could be:

ekf = EKF()
acc_data = imu.read_accelerometer_gyro_data()[:3]
q = ahrs.common.orientation.acc2q(acc_data)
print(q)    # Print Initial Orientation
while True:
    imu_data = imu.read_accelerometer_gyro_data()
    acc_data = imu_data[:3]
    gyr_data = imu_data[3:]
    q = ekf.update(q, gyr_data, acc_data)
    print(q)    # Print newest Orientation

Notice the index t is removed, because in your case you are giving a single sample of the sensors, not the full array with ALL measurements.

If you want to use the magnetometer samples, then I would suggest to substitute the acc2q function for the ecompass function, which estimates the initial orientation with the Accelerometer AND Magnetometer, and expand the loop to perform as:

ekf = EKF()
acc_data = imu.read_accelerometer_gyro_data()[:3]
mag_data = imu.read_magnetometer_data()
q = ahrs.common.orientation.ecompass(acc_data, mag_data, frame='ENU', representation='quaternion')
print(q)    # Print Initial Orientation
while True:
    imu_data = imu.read_accelerometer_gyro_data()
    acc_data = imu_data[:3]
    gyr_data = imu_data[3:]
    mag_data = imu.read_magnetometer_data()
    q = ekf.update(q, gyr_data, acc_data, mag_data)
    print(q)    # Print newest Orientation

Give it a try and let me know if it works.

[rhyshorner]

Hello,
thanks for your reply and with such detail.

oooh I see, with ekf = EKF(gyr=gyr_data, acc=acc_data) I should be passing in a full set of lists and the class would go through the full set and make a set of estimations them put them in Q

I modified my example script, it now looks like;

from icm20948 import ICM20948
import ahrs
from ahrs.filters import EKF

imu = ICM20948()
ekf = EKF()

mag_data = imu.mag_data_nplist()
acc_data, gyr_data = imu.acc_gyr_data_nplist()
q = ahrs.common.orientation.ecompass(acc_data, mag_data, frame='ENU', representation='quaternion')
print(q)    # Print Initial Orientation

while True:
    mag_data = imu.mag_data_nplist()
    acc_data, gyr_data = imu.acc_gyr_data_nplist()
    q = ekf.update(q, gyr_data, acc_data, mag_data)
    print(q)    # Print newest Orientation

traceback;

pi@raspberrypi:~/ICM20948 $ python3 mayitzin_and_icm20948.py 
[ 0.57840807  0.02587752 -0.01039307 -0.81527078]
Traceback (most recent call last):
  File "mayitzin_and_icm20948.py", line 18, in <module>
    q = ekf.update(q, gyr_data, acc_data, mag_data)
  File "/home/pi/ICM20948/ahrs/filters/ekf.py", line 1300, in update
    S   = H@[email protected] + self.R            # Measurement Prediction Covariance
ValueError: operands could not be broadcast together with shapes (6,6) (3,3) 
pi@raspberrypi:~/ICM20948 $ 

So it looks like ecompass function works and estimates the orientation, which can be seen from the print statement printing the initial orientation, it then has a q value to pass back into ekf.update()
but then it throws an error, I didnt even know @ was an operator for matrix multiplication before this error, learn something new every day.

I tried searching the value error to figure out the solution, but I dont think my programming, or mathematics, is advanced enough.

have you come across this or see an obvious mistake I have over looked?