ESP32-localization:

This projects aims to achieve the localization and mapping of an autonomous Rover or ROV.

Localization options:

Both options should output a set of data representing the position and orientation in 3d space, angular and linear velocities.

• Odometery: primary uses wheel encoders combined with IMU data. Used on ground vehicles as dead reckoning is prone to errors from acceleration sensors. linear errors occur as a result of uneven or slippery surfaces. Filters and sensor fusion can be applied.
  • Basics
  • More accurate derivation and equations

• Dead reckoning: relies on sensors that output acceleration, velocity, and direction. Integration drift error accumulates over time. Used in situation where encoders are impractical as water and air vehicles. Dead reckoning is the go as the wheel velocities are sent when a request is made from the esp.

BNO08x:

the BNO08x has on board sensor 32-bit arm cortex processor that runs sensor fusion algorithm to output real time, around 400hz. The sensor can output rotation vector, quaternion, gyro, accelerometer, and magnetometer data. Due to the limited numbers of pins, BNO will communicate using I2C Sparkfun library can support multiple sensors.

Equations for linear Kalman filter:

$$ P_{k∣k−1}​=P_{k−1∣k−1}​+Q $$

• $P_{k∣k−1​}$: predicted estimate covariance (error estimate) at time kk.
• $P_{k−1∣k−1}$: the updated error estimate from the previous step.
• $Q$: process variance. The Kalman gain determines how much the new measurement should adjust the estimate. It balances the trust between the prediction and the measurement.

$$ K_k = \frac{P_{K|k-1}}{P_{K|k-1}+R} $$

• $R$: measurement variance.

$$ X_{k|k} = X_{k|k-1} + K_k * (Z_k - X_{k|k-1}) $$

• $X_{k|k}$: The updated state estimate after incorporating the new measurement.
• $X_{k|k-1}$: The predicted state estimate.
• $Z_k$: The new measurement.
• $K_k$: The Kalman gain.

Updating the error estimate:

$$ P_{k∣k​}=(1−K_k​)P_{k∣k−1} $$

Sensor fusion and filtering:

• kalman filter: is the best option for linear systems. However, the change in velocity is not linear, nor the encoder's data with respect to the accelerometer.
• Extended Kalman filter: can be considered as another version of the linear kalman filter, but can handle non-linear systems. It is supported by the ESP-IDF DSP library, it is optimized to be efficient.
• Unscented kalman filter: can handle the cases where the non-linearity increases. However, it is computationally expensive. However, it is the way

Extended kalman filter:

Everything is similar with the Linear Kalman filter, however, this uses Taylor polinomials to normalize non-linear systems. This is the model and prediction step

Correction Steps

Problems to be addressed:

• regular kalman filter can be applied to yaw angle for regular IMUs. However, as I stated above, BNO does everything for us.
• slipping while rotating: can be corrected by fusing imu with odometry
• bumps and irregularities: add the acceleration to the kalman filter should decrease the problem, if not, orientation can be added.
• the acceleration on the z-axis can ruin the localization system, some of it will leak into other acceleration vectors.
• taring is essential, as it tells the BNO the inetial position.

Testing steps:

Each step adds a layer to the preceding one

• Step 0: Test IMU, calibration, taring, frequency
• Step 1: Test Odometry without the IMU
  • Step 1.1: Apply kalman filter to the Odometry
  • Step 1.2: Use Odometry for linear movements, IMU for angles
  • Step 1.3 Add IMU to the odometry Model
• Step 2: Test Dead Reckoning without the IMU
  • Step 2.1: Apply kalman filter for dead reckoning
  • Step 2.2: Use Dead Reckoning for linear movements, IMU for angles
  • Step 2.3: Fuse Odometry with IMU
  • Step 2.5: Add IMU to dead reckoning model
• Step 3: Merge accelerometer with the readings
• step 4: Add gps to the party.

TO DO: (Prioritized)

• Finish odom
• Finish deadReckoning
• BNO085 Setup
• Research Sensor Fusion
• implement sensor fusion
• Mapping techniques
• anti-slipping mechanism
• Optimize task handling

We are following a similar approach - excluding the camera - to the image above, the direction and calibration of the IMU are done automatically on the BNO board. Taring is done at the begining of the run, that is, reset the imu directions to the current state. Calibration of the wheels and motors shall be easier and straight forward, but it is crucial for straight driving and accurate measurements.

Important ressources:

This repo, and its explaination, provide a very close explaination of what we are trying to achieve, however, they do not account for bumps or irregularities in the track.

From the above resources, the part we

Resources:

• Unscented Kalman Filter for 6DoF IMU
• Extended Kalman Filter for position & orientation tracking on ESP32
• MPU driver
• Gaussian filter
• Unscented Kalman Filter(video)
• EKF esp32 example from espressif
• Filters + particle filter
• Open KF
• Encoders and IMU sensor Fusion
• How to fuse sensors stack exchange
• Quaternions-part 1
• Quaternions-part 2
• BNO085 library
• Visualizing Quaternions
• Ros robot localization package, the package itself is not docuemnted, not recommended overall
• Sensor fusion full course