Add a part to produce pose estimates using an IMU

[TCIII]

Create a part to produce pose estimates use acc/gyro/mag values from an IMU. Output is pose (x, y, angle) and accelerations (x_d, y_d, angle_d) where (x, y) is position in meters, x-axis is positive in the east direction, y axis is positive in north direction and angle is the orientation of the vehicle in radians where zero degrees is east and angles increase counterclockwise. The output should be in the same format as the kinematics pose estimators;
returns a tuple whose contents is:

• distance: total forward distance traveled
• velocity: forward velocity
• x: is horizontal position of reference point midway between front wheels
• y: is vertical position of reference point midway between front wheels
• angle: is orientation in radians of the vehicle along it's wheel base (along the line between the reference points midway between the front wheels and and midway between the back wheels)
• x': is the horizontal velocity (rate of change of reference point along horizontal axis)
• y': is the vertical velocity (rate of change of reference point along vertical axis)
• angle': is the angular velocity (rate of change of orientation)
• timestamp: is the time is seconds for the reading

We support 6 degrees of freedom, like MPU6050 (3-axis accelerometer and 3 axis gyro) and 9 degrees of freedom, like MPU9250 (3-axis accelerometer, 3 axis gyro, 3 axis magnetometer).

Include unit tests. Hint: make sure the run() method takes an optional timestamp in addition to the IMU values; if this is None the part should use time.time(), otherwise use the provided time. That allows unit tests to control the time.

Integrate this as a source of pose in the path_follow template and measure it's accuracy.

[Ezward]

Here are some links for integrating IMU and for fusing IMU with GPS

• https://pypi.org/project/droneposelib/. dronepose lib is a python library that will make pose estimates based on IMU data.
• https://pypi.org/project/imusensor/ library to make reading the IMU easy. Look at the acknowledgements at the bottom of the page.
• https://github.com/micropython-IMU/micropython-fusion micro-python library implements Madgwick
• https://github.com/xioTechnologies/Fusion AHRS fusion in C with Python bindings. Includes some error rejection.
• https://github.com/smahajan07/sensor-fusion Python fuse GPS with IMU for position and orientation estimation
• https://www.youtube.com/watch?v=6M6wSLD-8M8 video explaining IMU/GPS fusion with Kalman filter
• https://github.com/slobdell/kalman-filter-example code for fusing IMU/GPS in go lang.

[Ezward]

Here is some python code I generated with ChatGPT to use an MPU9250 to generate pose. Note that I specifically asked to use the mpu9250_jmdev package because that is what we have installed in Donkeycar already, but perhaps there is a better package?

CAUTION: this code was generated by ChatGPT has not been tested and almost certainly has at least small errors, like not specifying an import package for instance. The same goes for all the examples included in subsequent comments.

#
# Q: write Python code using the mpu9250_jmdev package to do dead reckoning with an MPU9250 imu and include orientation as roll, pitch, yaw
# A: Here's an example of how you can use the mpu9250_jmdev package to do dead reckoning with an MPU9250 IMU and include orientation information as roll, pitch, and yaw.
# This code initializes the MPU9250, sets the orientation to landscape, and sets the sample rate to 100 Hz. It then uses a loop to continuously integrate the acceleration and angular velocity to get the velocity and position. # The orientation is represented as a quaternion and is converted to roll, pitch, and yaw using the quaternion_to_euler_angles function from the mpu9250_jmdev package. The position, velocity, and orientation information are then printed.
#
import mpu9250_jmdev
import time
import numpy as np

# Initialize the MPU9250
imu = mpu9250_jmdev.IMU()

# Set the sample rate to 100 Hz
imu.mpu.requested_rate = 100

# Set the orientation to landscape
imu.orientation = mpu9250_jmdev.Orientation.LANDSCAPE

# Initialize variables for position, velocity, and orientation
pos = np.array([0.0, 0.0, 0.0])
vel = np.array([0.0, 0.0, 0.0])
quat = np.array([1.0, 0.0, 0.0, 0.0])

# Get the time of the first measurement
last_time = time.time()

while True:
    # Get the current acceleration, angular velocity, and orientation
    accel = imu.acceleration
    gyro = imu.gyro
    quat = imu.quaternion

    # Convert the orientation to roll, pitch, yaw
    roll, pitch, yaw = mpu9250_jmdev.quaternion_to_euler_angles(quat)

    # Get the current time
    curr_time = time.time()
    dt = curr_time - last_time

    # Integrate the acceleration to get the velocity
    vel = vel + accel * dt

    # Integrate the velocity to get the position
    pos = pos + vel * dt

    # Do something with the position, velocity, and orientation information
    print("Position:", pos)
    print("Velocity:", vel)
    print("Orientation (roll, pitch, yaw):", roll, pitch, yaw)

    # Update the last time
    last_time = curr_time

    # Wait for the next measurement
    time.sleep(0.01)

[Ezward]

Here is code that only assumes a a 6 axis acc/gyro like an MPU6050 (so no magnetometer). In integrates (x, y, z) position. Note that we can simply use prior position and the new position to estimate the orientation.

The code is in Rust, but it's obvious what it is doing.

//
// Q: Write rust code for the Raspberry Pi Pico to do dead reckoning with an MPU9250 imu
// A: Here's an example implementation in Rust that implements dead reckoning using an MPU9250 IMU on a Raspberry Pi Pico:
//
use embedded_hal::blocking::delay::DelayMs;
use embedded_hal::spi::{Mode, Phase, Polarity};
use mpu9250::{Mpu9250, SlaveAddr};
use raspberry_pico::{CorePeripherals, Peripherals};
use rppal::spi::Spi;
use std::thread;
use std::time::Duration;

pub fn run(peripherals: Peripherals, core: CorePeripherals) {
    let spi = Spi::new(peripherals.SPI0, SlaveAddr::default(), Mode::Mode0, 8.mhz(), Polarity::IdleLow, Phase::CaptureOnFirstTransition).unwrap();
    let delay = raspberry_pico::Delay {};
    let mut mpu9250 = Mpu9250::new(spi, delay).unwrap();
    mpu9250.accel_config().unwrap();
    mpu9250.gyro_config().unwrap();

    let mut x = 0.0;
    let mut y = 0.0;
    let mut z = 0.0;
    let mut vx = 0.0;
    let mut vy = 0.0;
    let mut vz = 0.0;
    let dt = 0.01;

    loop {
        let accel_data = mpu9250.accel().unwrap();
        let gyro_data = mpu9250.gyro().unwrap();

        let ax = accel_data.x as f32 / 9.80665;
        let ay = accel_data.y as f32 / 9.80665;
        let az = accel_data.z as f32 / 9.80665;

        let gx = gyro_data.x as f32 * 0.0174533;
        let gy = gyro_data.y as f32 * 0.0174533;
        let gz = gyro_data.z as f32 * 0.0174533;

        vx += ax * dt;
        vy += ay * dt;
        vz += az * dt;

        x += vx * dt + 0.5 * ax * dt * dt;
        y += vy * dt + 0.5 * ay * dt * dt;
        z += vz * dt + 0.5 * az * dt * dt;

        thread::sleep(Duration::from_millis((dt * 1000.0) as u64));
    }
}

[Ezward]

Here is an alternative the explicity calculates orientation (pitch, roll, yaw) by integrating the gyro. Note this is not based on a specific library.

#
# Q: write Python code to do dead reckoning with an MPU9250 imu including magnetometer and print (x, y, z) position, (ax, ay, ax) velocity and orientation; print orientation as roll, pitch, yaw.
# A: Here is an example code in Python for dead reckoning with an MPU9250 IMU including magnetometer:
# Note that this code assumes that you have an MPU9250 library that provides functions to read the accelerometer, gyro, and magnetometer data. The code also assumes that the correct orientation can be obtained from the magnetometer data, but the implementation of this correction is not shown here.
#
import time
import math

# Assuming MPU9250 library has been imported

# initialize MPU9250
imu = MPU9250()
imu.initialize()

# Set initial position, velocity, and orientation
x, y, z = (0, 0, 0)
vx, vy, vz = (0, 0, 0)
roll, pitch, yaw = (0, 0, 0)

# Set delta time (time between updates)
delta_t = 0.01 # seconds

while True:
    # Read accelerometer, gyro, and magnetometer data as 3 element arrays (x, y, z)
    accel = imu.getAccel()
    gyro = imu.getGyro()
    mag = imu.getMag()
    
    # Convert accelerometer data to position
    x += vx * delta_t + 0.5 * accel[0] * delta_t**2
    y += vy * delta_t + 0.5 * accel[1] * delta_t**2
    z += vz * delta_t + 0.5 * accel[2] * delta_t**2
    
    # Convert gyro data to orientation
    roll += gyro[0] * delta_t
    pitch += gyro[1] * delta_t
    yaw += gyro[2] * delta_t
    
    # Correct orientation using magnetometer data (not shown here)
    
    # Update velocity based on accelerometer data
    vx += accel[0] * delta_t
    vy += accel[1] * delta_t
    vz += accel[2] * delta_t
    
    # Print position, velocity, and orientation
    print("Position: ({:.2f}, {:.2f}, {:.2f})".format(x, y, z))
    print("Velocity: ({:.2f}, {:.2f}, {:.2f})".format(vx, vy, vz))
    print("Orientation: Roll={:.2f}, Pitch={:.2f}, Yaw={:.2f}".format(roll, pitch, yaw))
    
    time.sleep(delta_t)

[Ezward]

code to convert a quaternion to euler angles (pitch, roll, yaw)

#
# Q: Write the quat2euler function required in the previous response
# A: Here's the quat2euler function that can be used to convert quaternion representation of orientation to roll, pitch, yaw representation:
# 
def quat2euler(w, x, y, z):
    roll = math.atan2(2 * (w * x + y * z), 1 - 2 * (x**2 + y**2))
    pitch = math.asin(2 * (w * y - z * x))
    yaw = math.atan2(2 * (w * z + x * y), 1 - 2 * (y**2 + z**2))
    return (roll, pitch, yaw)

[Ezward]

Here is code that used the Madgewick algorithm to fuse acc/gyro/mag into a more accurate orientation. This can be combined with prior examples and integrate acc to produce position so we can estimate a full (x, y, orientation) pose with a more accurate orientation. Note that the Madgewick.update() method is not in the mpu9250 package that we used. There are a number of python implementations (see comment above). This package includes a version that can handle magnetometer or no magnetometer; Note that he For instance, this micropython package is more recent and include the Madgewick algorithm to calculate a better orientation using the magnetometer/gyro/acc fusion; https://github.com/micropython-IMU/micropython-fusion/blob/master/fusion.py

#
# Q: write Python code using the mpu9250_jmdev package to do dead reckoning with an MPU9250 imu including magnetometer and print position, velocity and orientation; print orientation as roll, pitch, yaw.
# A: Here's an example code in Python that demonstrates how to do dead reckoning with an MPU9250 imu including magnetometer and print position, velocity, and orientation as roll, pitch, yaw:
# Note: This code assumes you have already installed the mpu9250_jmdev package
#
import mpu9250_jmdev
import time

mpu = mpu9250_jmdev.MPU9250()

try:
    while True:
        accel = mpu.read_accel_data()
        gyro = mpu.read_gyro_data()
        mag = mpu.read_mag_data()

        # Use the Madgwick filter to calculate orientation
        orientation = Madgwick.update(gyro['x'], gyro['y'], gyro['z'], accel['x'], accel['y'], accel['z'], mag['x'], mag['y'], mag['z'])

        # Calculate roll, pitch, yaw
        roll = orientation[0]
        pitch = orientation[1]
        yaw = orientation[2]

        # Print the results
        print("Accelerometer data: X={0}, Y={1}, Z={2}".format(accel['x'], accel['y'], accel['z']))
        print("Gyroscope data: X={0}, Y={1}, Z={2}".format(gyro['x'], gyro['y'], gyro['z']))
        print("Magnetometer data: X={0}, Y={1}, Z={2}".format(mag['x'], mag['y'], mag['z']))
        print("Orientation: Roll={0}, Pitch={1}, Yaw={2}".format(roll, pitch, yaw))

        # Wait for some time
        time.sleep(0.5)

except KeyboardInterrupt:
    pass

[Ezward]

Note that the prior examples do not include necessary calibration. See this issue #1071

[Ezward]

import mpu9250_jmdev
import time

# Initialize the MPU9250
mpu = mpu9250_jmdev.MPU9250()

# Initialize variables for position, velocity, and orientation
position = [0.0, 0.0, 0.0]
velocity = [0.0, 0.0, 0.0]
orientation = [0.0, 0.0, 0.0]

# Get the time of the first measurement
last_time = time.time()

try:
    while True:
        # Get the accelerometer, gyroscope, and magnetometer data
        accel = mpu.get_accel_data()
        gyro = mpu.get_gyro_data()
        mag = mpu.get_mag_data()

        # Get the current time
        curr_time = time.time()
        dt = curr_time - last_time

        # Integrate the acceleration to get the velocity
        velocity[0] += accel['x'] * dt
        velocity[1] += accel['y'] * dt
        velocity[2] += accel['z'] * dt

        # Integrate the velocity to get the position
        position[0] += velocity[0] * dt
        position[1] += velocity[1] * dt
        position[2] += velocity[2] * dt

        # Get the orientation as roll, pitch, yaw
        orientation = mpu9250_jmdev.quaternion_to_euler_angles(mpu.get_orientation())

        # Print the results
        print("Position: ({:.2f}, {:.2f}, {:.2f})".format(position[0], position[1], position[2]))
        print("Velocity: ({:.2f}, {:.2f}, {:.2f})".format(velocity[0], velocity[1], velocity[2]))
        print("Orientation: Roll={:.2f}, Pitch={:.2f}, Yaw={:.2f}".format(orientation[0], orientation[1], orientation[2]))

        # Update the last time
        last_time = curr_time

        # Wait for some time
        time.sleep(0.1)

except KeyboardInterrupt:
    pass