🚀 Development of a Positioning System Using IMU and

This project implements an Inertial Navigation System (INS) using a 9-DoF IMU (ICM-20948) and an ESP32.
The system performs sensor calibration, fusion for attitude estimation, linear acceleration computation, and position estimation using ZVU.

---

1. Attitude & Sensor Fusion (AHRS)

The core of this code relies on the Fusion AHRS algorithm.

a) Raw Data Calibration

Before data enters the fusion filter, raw sensor readings are converted into standard physical units.

Sensor	Raw Input	Conversion Formula
Gyroscope	myICM.gyrX() (Raw Counts)	Gyroscope (DPS) = Raw - Bias
Accelerometer	data.Raw_Accel.Data.X (Raw Counts)	a_calib = (Sensitivity * a_raw - b_accel) ⊙ S * G

Where:

• a_calib: calibrated acceleration (m/s²)
• a_raw: raw acceleration vector (Raw Counts)
• b_accel: bias vector
• S: scale factor vector
• Sensitivity = (FSR * g) / 32767.5
• ⊙: Hadamard (element-wise) product

---

b) Prediction Stage

• Gyroscope: Measures angular rates. Integration over time updates sensor orientation q:

$$q^. = 0.5 * q * w_q$$ $$w_q = [0,w_x,w_y,w_z]$$ $$q_t ≈ q_{t-1} + 0.5 * q_{t-1} ⊗ ω_t*Δt$$

c) Correction Stage

• Accelerometer: Measures gravity for drift correction (Roll & Pitch)

The accelerometer measures the direction of gravity and we get the predicted direction of gravity using the predicted quaternion and compare it with the accelerometer and get an error Gravity vector in the sensor frame :

$$g(q) = q ⊗ [0,0,0,1] * q^*$$ $$e = a × g(q)$$

Why the outer product? 1 ) e is perpendicular to both a and g(q) This is the axis we rotate around to fit a onto g(q) 2) Since the magnitude of a and g(q) is 1,

$$||e|| = Sinθ$$

The larger θ, the more error we produce

---

2. Earth Frame Linear Acceleration

a) Rotation to Earth Frame

The accelerometer measures sensor-frame acceleration. To remove gravity:

$$a_{earth} = R * a_{sensor}$$

• R: Rotation matrix obtained from quaternions.

b) Moving Average Filter (MA)

Reduces high-frequency noise:

$$a_{filtered}[n] = \frac{1}{N} \sum_{i=0}^{N-1} a_{linear\_earth}[n-i], \quad N = 15$$

---

3. Position Estimation with Zero Velocity Update (INS/ZVU)

This section is the core of the INS (Inertial Navigation System) method based on PDR (Pedestrian Dead Reckoning) which works using the ZVU technique to deal with drift.

a) Trapezoidal Integration

To update the position and velocity, the trapezoidal integration method is used (which is more accurate than the simple Euler integration):

$$v_t = v_{t-1} + 0.5 * (a_t + a_{t-1}) * \Delta t$$ $$p_t = p_{t-1} + 0.5 * (v_t + v_{t-1}) * \Delta t$$

b) Zero Velocity Update (ZVU) method

The position drift in the INS is caused by the accumulation of small errors in a and Δt. ZVU assumes that during a short stop, the velocity and acceleration must be zero and uses this assumption to reset the accumulated error.

For each axis (X, Y, Z), the filtered linear acceleration in Earth Frame is compared to a threshold:

• X, Y: 0.5 m/s²
• Z: 0.8 m/s²

$$\text{If } |a_{filtered}| > Threshold \Rightarrow \text{Motion detected, continue integration, ZVU counter = 0}$$

If the ZVU_{counter}counter crosses the threshold for a certain number of samples, a complete stop is assumed and the speed of that axis is explicitly set to zero:

$$\text{If } ZVU_{counter} >= 3 \Rightarrow \text{v=0}$$

Of course, it should be noted that using the IMU alone will not provide very good positioning accuracy because this method depends on creating acceleration from a threshold, and as a result, positioning cannot be done with the IMU alone for very slow movements because when stationary, the IMU accelerometer also has a value for acceleration in each axis, and with slow movements, it becomes difficult to detect and differentiate between no movement and slow movement, and this is a big challenge.

---

Output and results

This video is to demonstrate repeatability, with a small error of 2 cm.

The next video is about the movement in the z-axis direction, which can be seen moving in other axes. Of course, in subsequent updates, the drift in other axes was reduced, but unfortunately I don't have a video of it.