Mayitzin / ahrs

Attitude and Heading Reference Systems in Python
https://ahrs.readthedocs.io/
MIT License
566 stars 91 forks source link

How ist measurement noise defined? #43

Open AlexVaith opened 3 years ago

AlexVaith commented 3 years ago

I try to use the extended Kalman filter. I have the datasheet of the sensors being used, but I struggle to identify the specific values the ekf() function is looking for.

I am a newbie to sensors and datasheets in general. Could you help me to identify the values for the following sensor: ICM-20649.

I get the following values for the accelerometer: RMS Noise [mg-rms] TYP (based on accelerometer noise: 285μg/√Hz)

and for the gyroscope: RMS Noise [dps-rms] TYP (based on gyroscope noise: 0.0175dps/√Hz)

and for the magnetometer: 3.2mgauss.

Thanks in advance

PeterBorisenko commented 1 year ago

Hello. Taking a sensor datasheet variance or noise density is not enough to build a robust estimation. This is mostly because datasheet data represents sensor itself and not a system it works onto. The system (vehicle, tool, etc.) can introduce vibrations which impact sensor's data. So the sensor data analysis is greatly advised

The way I do it is the following:

  1. sample enough sensor data while all main components of your system are turned on (engines ON and so on) but the system itself is not moving
  2. calculate covariance matrix (using a python script or excel)
  3. use this covariance matrix in EKF

An example of python code (assuming your IMU/Compass data is stored in csv file as 9 columns - 3 for each sensor):

import numpy as np
import matplotlib.pyplot as plt
import csv

ACCELEROMETER_DATA_POS= 0
GYROSCOPE_DATA_POS= 3
MAGNETOMETER_DATA_POS= 6

def plotDistribution(dataset, datasetName):
    (dsNum, dsVals)= dataset

    plt.suptitle("{} distribution".format(datasetName))

    for i in range(3):
        counts, bins = np.histogram(dsVals[:,i] - np.mean(dsVals[:,i]))
        plt.stairs(counts/dsNum, bins)

    plt.grid()
    plt.xlim([-0.15, 0.15])
    plt.ylim([0, 1])
    plt.show()

def printCovarianceMatrix(dataset, name):
    print("{} Covariance matrix:\n{}".format(name, np.cov(dataset.T)))

def getData(filename):
    datafile= open(filename, mode='r')
    reader= csv.reader(datafile)
    rows = list(reader)
    datafile.close()

    a= np.empty(3)
    g= np.empty(3)
    m= np.empty(3)

    for row in rows:
        a= np.vstack((a, np.array([float(i) for i in row[ACCELEROMETER_DATA_POS : ACCELEROMETER_DATA_POS+3]])))
        g= np.vstack((g, np.array([float(i) for i in row[GYROSCOPE_DATA_POS : GYROSCOPE_DATA_POS + 3]])))
        m= np.vstack((m, np.array([float(i) for i in row[MAGNETOMETER_DATA_POS : MAGNETOMETER_DATA_POS + 3]])))

    a= a[1:]
    g= g[1:]
    m= m[1:]
    num_samples= len(rows)
    timeline= np.linspace(0,num_samples,num=num_samples)
    return {'n':num_samples, 'a':a, 'g':g, 'm':m, 't':timeline}

sensData= getData('./SensorNoise(MotorsON).csv')

printCovarianceMatrix(sensData['a'], "Accelerometer")
plotDistribution((sensData['n'], sensData['a']), 'Accelerometer noise')
printCovarianceMatrix(sensData['g'], "Gyroscope")
plotDistribution((sensData['n'], sensData['g']), 'Gyroscope noise')
printCovarianceMatrix(sensData['m'], "Magnerometer")
plotDistribution((sensData['n'], sensData['m']), 'Magnerometer noise')

Hope it'll be useful