**Lab 6** | IMU Sensors, Noise Analysis, Extended Kalman Filter ### Design of Autonomous Systems ### CSCI 6907/4907 - Section 86 ### Prof. **Sibin Mohan** --- ## objectives - understand all IMU (Inertial Measurement Unit) sensors - and measurement functions - sensor noise characteristics, sources, mitigation techniques - filtering algorithms+calibration procedures for noise reduction - understand **EKF** for sensor fusion - apply EKF for real-world robotic applications --- ## What is an IMU? An **IMU (Inertial Measurement Unit)** is an electronic device that measures specific force, angular rate, and magnetic field using a combination of accelerometers, gyroscopes, and magnetometers. - Electronic device that measures specific force, angular rate, and magnetic field - Self-contained system; no external references required - Provides continuous motion data at high sampling rates - **Intel RealSense D435i** incorporates a built-in **BMI055 IMU** (Bosch) **Primary Applications:** - Dead reckoning navigation - Orientation estimation - Motion tracking and stabilization - Sensor fusion for localization --- ## IMU Sensor Suite: 9-DOF System **9-DOF System:** 3 axes x 3 sensors = 9 **DOF** (Degrees of Freedom) | Sensor | Measures | Unit | Primary Use | Key Limitation | |--------|----------|------|-------------|----------------| | **Accelerometer** | Linear acceleration | $m/s^2$ or $g$ | Tilt detection | Cannot distinguish gravity from motion | | **Gyroscope** | Angular velocity | $^\circ$/s or $\text{rad}/s$ | Rotation tracking | Drifts over time | | **Magnetometer** | Magnetic field | $\mu\,T$ or gauss ($G$) | Absolute heading | Susceptible to interference | --- ## IMU Measurement Examples **Integration:** $\theta(t) = \theta_0 + \int \omega \, dt$ (causes drift over time) | Condition | Accel X | Accel Y | Accel Z | Gyro Z | |-----------|---------|---------|---------|--------| | Flat on table | 0 | 0 | $+9.81\,m/s^2$ | $0^\circ$/s | | Free-fall | 0 | 0 | 0 | $0^\circ$/s | | Rotating **CCW** (Counter-Clockwise) at 30$^\circ$/s | -- | -- | -- | $+30^\circ$/s | --- ## Sources of IMU Sensor Noise | Source | Description | Typical Values | |--------|-------------|----------------| | **Thermal Noise** | Random electron movement (Johnson-Nyquist noise) | Accel: 150-200 $\mu\,g/\sqrt{Hz}$; Gyro: 0.007-0.01 $^\circ$/s/$\sqrt{Hz}$ | | **Vibration Noise** | Mechanical vibrations from motors/terrain | Frequency bands related to motor **RPM** | | **EMI** (Electromagnetic Interference) | External fields from power lines, motors | Particularly affects magnetometers | | **Quantization Noise** | Error from **ADC** (Analog-to-Digital Conversion) | Depends on bit resolution | --- ## Measurement Error Types | Error Type | Description | Impact | |------------|-------------|--------| | **Bias** | Constant offset; output not zero when input is zero | Gyro bias 0.1 $^\circ$/s -> 6 $^\circ$ error after 1 min, 360 $^\circ$ after 1 hour | | **Bias Instability** | Random variation in bias value over short periods | Measured in $^\circ$/h or $^\circ$/s/$\sqrt{Hz}$; limits long-term accuracy | | **Scale Factor Errors** | Multiplicative error; sensor over/under-reports | Proportional to input magnitude | | **Cross-Axis Sensitivity** | Sensors respond to orthogonal axis inputs | Coupling between axes | --- ## Impact of Noise on Dead Reckoning **Position by Double Integration:** $$p(t) = p_0 + v_0 t + \iint a \, dt^2$$ **Error Propagation:** - Accelerometer bias -> position error $\propto t^2$ - Gyroscope bias -> orientation error $\propto t$ - Random walk -> errors $\propto t^{3/2}$ **Example:** 0.01 $m/s^2$ accelerometer bias --- ## Filtering Methods | Filter | Purpose | Equation | |--------|---------|----------| | **Low-Pass** | Remove high-frequency noise | $y_k = \alpha \cdot y_{k-1} + (1 - \alpha) \cdot x_k$ | | **Complementary** | Combine gyro (short-term) + accel (long-term) | $\theta_k = \alpha \cdot (\theta_{k-1} + \omega_k \Delta t) + (1 - \alpha) \cdot \theta_{accel}$ | Where $\alpha$ = smoothing factor (0.8-0.95 for low-pass, ~0.98 for complementary) --- ## Calibration Procedures **Bias Calibration (Static Method):** 1. Place IMU in known orientation (flat and level) 2. Record samples (e.g., 1000 at 100 $Hz$) 3. Compute mean: $b_a = \frac{1}{N} \sum_{i=1}^{N} a_i$ 4. Subtract bias: $a_{corrected} = a_{raw} - b_a$ **Scale and Cross-Axis Calibration:** - Calibration matrix: $\mathbf{a}_{corrected} = \mathbf{M} \cdot \mathbf{a}_{raw} - \mathbf{b}$ - **Magnetometer:** Hard iron (offset) + Soft iron (ellipsoid to sphere) --- ## Why Sensor Fusion? **No Single Sensor is Perfect:** | Sensor | Good For | Problems | |--------|----------|----------| | Accelerometer | Long-term tilt | Noisy, affected by motion | | Gyroscope | Short-term rotation | Drifts over time | | Magnetometer | Absolute heading | Noisy, interference | --- ## Why EKF for IMU? **EKF is the Standard for IMU Sensor Fusion:** 1. **Optimality:** Best linear unbiased estimate for Gaussian noise 2. **Efficiency:** Recursive formulation for real-time systems 3. **Uncertainty Quantification:** Maintains covariance estimates 4. **Multi-Sensor Fusion:** Handles different update rates 5. **Non-Linear Handling:** Extends standard Kalman Filter via linearization --- ## EKF Applications - **AHRS** (Attitude and Heading Reference Systems) - **VIO** (Visual-Inertial Odometry) - **GPS-INS** (Global Positioning System - Inertial Navigation System) integration - **SLAM** (Simultaneous Localization and Mapping) --- ## EKF Mathematical Foundation **The Non-Linear System Model:** $$ \begin{aligned} \text{State Transition:} \quad & x_k = f(x_{k-1}, u_k) + w_k \\ \text{Measurement Model:} \quad & z_k = h(x_k) + v_k \end{aligned} $$ **Jacobian Definition:** $$ \mathbf{F}_k = \left.\frac{\partial f}{\partial x}\right|_{\hat{x}_{k\vert k-1}}, \quad \mathbf{H}_k = \left.\frac{\partial h}{\partial x}\right|_{\hat{x}_{k\vert k-1}} $$ Where: - $x_k$ = current state, $x_{k-1}$ = previous state - $u_k$ = control input, $w_k$ = process noise (covariance $Q$) - $z_k$ = measurement, $v_k$ = measurement noise (covariance $R$) - $\mathbf{F}_k$ = Jacobian of $f$, $\mathbf{H}_k$ = Jacobian of $h$ - $\hat{x}_{k\vert k-1}$ = predicted state --- ## EKF Prediction Step ### Step 1: Prediction | Operation | Equation | |-----------|----------| | State Prediction | $\hat{x}_{k\vert k-1} = f(\hat{x}_{k-1\vert k-1}, u_k)$ | | Covariance Prediction | $P_{k\vert k-1} = F_k P_{k-1\vert k-1} F_k^T + Q_k$ | --- ## EKF Update Step ### Step 2: Update | Operation | Equation | |-----------|----------| | Innovation (Residual) | $y_k = z_k - h(\hat{x}_{k\vert k-1})$ | | Kalman Gain | $K_k = P_{k\vert k-1} H_k^T (H_k P_{k\vert k-1} H_k^T + R_k)^{-1}$ | | State Update | $\hat{x}_{k\vert k} = \hat{x}_{k\vert k-1} + K_k y_k$ | | Covariance Update | $P_{k\vert k} = (I - K_k H_k) P_{k\vert k-1}$ | --- ## EKF State Vector **Typical 10-State Vector:** $$x = \begin{bmatrix} q_w & q_x & q_y & q_z & b_{gx} & b_{gy} & b_{gz} & b_{ax} & b_{ay} & b_{az} \end{bmatrix}^T$$ **Components:** - $q_w, q_x, q_y, q_z$ = quaternion (4-parameter construct for 3D rotation) - $b_{gx}, b_{gy}, b_{gz}$ = gyroscope bias - $b_{ax}, b_{ay}, b_{az}$ = accelerometer bias --- ## EKF Process Model **Quaternion Update** (using gyroscope): $$q_k = q_{k-1} \otimes \begin{bmatrix} 1 \\ \frac{1}{2}(\omega_k - b_g) \Delta t \end{bmatrix}$$ **Bias Model** (Random Walk): $$b_{g,k} = b_{g,k-1} + w_{bg}, \quad b_{a,k} = b_{a,k-1} + w_{ba}$$ **Prediction Rate:** IMU rate (100-1000 $Hz$) --- ## EKF Measurement Updates **Accelerometer Update** (when not accelerated): $$h_a(x) = R(q)^T \cdot g \quad \text{where } g = [0, 0, 9.81]^T$$ **Magnetometer Update:** $$h_m(x) = R(q)^T \cdot m \quad \text{where } m = \text{local magnetic field}$$ --- ## Multi-Rate Sensor Fusion | Sensor | Rate | Action | |--------|------|--------| | **IMU** | 100-1000 $Hz$ | Prediction only | | **Magnetometer** | 10-100 $Hz$ | Update when available | | **GPS** | 1-10 $Hz$ | Update when available | EKF naturally handles this by running prediction at highest rate and performing updates when measurements arrive. --- ## EKF Implementation Considerations 1. **Quaternion Normalization:** $q \leftarrow q / ||q||$ after each update 2. **Covariance Initialization:** Set $P_0$ to reflect initial uncertainty 3. **Noise Tuning:** - **Process Noise Q:** Larger = trust measurements more - **Measurement Noise R:** Reflects actual sensor noise 4. **Outlier Rejection:** Mahalanobis distance gating --- ## ROS Implementation ```bash # Install robot_localization package sudo apt install ros-noetic-robot-localization # Launch EKF roslaunch robot_localization ekf_template.launch ``` **Key Parameters:** - `odom_frame`: Frame for odometry output - `world_frame`: Global reference frame - `sensor_timeout`: Maximum acceptable delay - `two_d_mode`: Constrain to 2D plane --- ## Steering Models Overview | Model | Key Features | Turning | In-Place | Strafe | Use Case | |-------|--------------|---------|----------|--------|----------| | **Ackermann** | Front wheels turn at different angles | Variable (depends on steering angle) | No | No | Cars, autonomous vehicles | | **Differential** | Two independently powered wheels | Variable | Yes | No | Mobile robots, warehouse robots | | **Omnidirectional** | Mecanum/spherical wheels | None | Yes | Yes | Service robots, industrial robots | **Key Considerations:** Navigation capabilities, maneuverability, energy efficiency (Ackermann > Differential > Omnidirectional), control complexity --- ## Ackermann Steering Kinematics $$x_{t+1} = x_t + v \cos(\theta) \Delta t$$ $$y_{t+1} = y_t + v \sin(\theta) \Delta t$$ $$\theta_{t+1} = \theta_t + \frac{v}{L} \tan(\delta) \Delta t$$ Where: $v$ = velocity, $\delta$ = steering angle, $L$ = wheelbase --- ## Differential Drive Kinematics $$x_{t+1} = x_t + \frac{r}{2} (\omega_R + \omega_L) \cos(\theta) \Delta t$$ $$y_{t+1} = y_t + \frac{r}{2} (\omega_R + \omega_L) \sin(\theta) \Delta t$$ $$\theta_{t+1} = \theta_t + \frac{r}{d} (\omega_R - \omega_L) \Delta t$$ Where: $r$ = wheel radius, $d$ = wheel distance, $\omega$ = angular velocities --- ## Omnidirectional Kinematics $$x_{t+1} = x_t + v_x \Delta t$$ $$y_{t+1} = y_t + v_y \Delta t$$ $$\theta_{t+1} = \theta_t + \omega \Delta t$$ --- ## Kinematic Reachability | Model | Turning Radius | Rotate in Place? | Can Strafe? | |-------|---------------|------------------|-------------| | Ackermann | Variable (depends on steering angle) | No | No | | Differential | Variable | Yes | No | | Omnidirectional | None | Yes | Yes | **Kinematic Reachability:** Set of positions reachable based on steering constraints (without velocity/acceleration) --- ## Dynamic Reachability **Dynamic Reachability:** Expands kinematic reachability by incorporating velocity, acceleration, and slip constraints **Key Factors:** - **Friction Constraints:** High-speed turns -> drift or loss of control - **Actuator Limits:** Maximum acceleration restricts maneuverability - **Energy Efficiency:** Ackermann > Differential > Omnidirectional - **Slip and Skid:** Fast motions impact accuracy and stability --- ## Ackermann Steering: Setup **Environment Setup:** ```bash # Install dependencies sudo apt install ros-<distro>-rosserial ros-<distro>-joy ros-<distro>-ackermann-msgs # Create ROS package cd ~/catkin_ws/src catkin_create_pkg ackermann_steering std_msgs rospy geometry_msgs ackermann_msgs ``` Reference: [Ackermann Steering Controller](https://wiki.ros.org/ackermann_steering_controller) --- ## ROS Node Implementation - Subscribe to /cmd_vel or /ackermann_cmd - Calculate inner and outer wheel angles using Ackermann equations - Publish angles to servo motor controlling front wheels - Drive rear wheels using **PWM** (Pulse Width Modulation) or motor driver commands --- ## PWM Control - **Servo Steering:** Use Navio's PWM outputs, map steering angles to PWM values - **Motor Control:** Convert speed commands to throttle PWM values - **Calibration:** Center position, angle limits, deadband compensation --- ## Testing the Node ```bash # Run ROS Node rosrun ackermann_steering steering_controller.py # Publish Test Commands rostopic pub /cmd_vel geometry_msgs/Twist "{linear: {x: 1.0}, angular: {z: 0.5}}" ``` **Expected Result:** Rover moves forward with a turn --- ## Testing Checklist - [ ] Straight line driving - [ ] Various turn radii - [ ] Figure-8 patterns - [ ] Emergency stop response --- ## Further Improvements | Area | Actions | |------|---------| | **PID** Tuning | Tune for steering accuracy, minimize oscillation, improve response time | | **Sensor Fusion** | Integrate IMU data, add wheel encoders, improve state estimation | | **Parameter Optimization** | Adjust wheelbase, track width; calibrate for real-world performance | | **Safety** | Speed limiting in turns, obstacle detection integration, fail-safe behaviors | --- ## Key Takeaways: IMU Sensors - **Accelerometer:** Linear acceleration + gravity (~$9.81\,m/s^2$ when stationary) - **Gyroscope:** Angular velocity (drifts over time) - **Magnetometer:** Magnetic field (absolute heading) --- ## Key Takeaways: Sensor Noise - **Sources:** Thermal, vibration, EMI, quantization - **Errors:** Bias, scale factor, cross-axis sensitivity - **Impact:** Position error $\propto t^2$ for accelerometers **Noise Mitigation:** - Low-pass filtering removes high-frequency noise - Calibration compensates for systematic errors - Sensor fusion combines multiple sensors optimally --- ## Key Takeaways: Steering Models | Model | Turning | In-Place | Strafe | Use Case | |-------|---------|----------|--------|----------| | Ackermann | Variable | No | No | Cars | | Differential | Variable | Yes | No | Mobile robots | | Omnidirectional | None | Yes | Yes | Service robots | --- ## Key Takeaways: EKF - **Two-step process:** Prediction + Update - **Handles non-linearity** via Jacobian linearization - **Optimal estimation** with quantified uncertainty **Applications:** - AHRS, VIO, GPS-INS integration, SLAM - **Critical for:** Autonomous navigation in GPS-denied environments --- ## References [1] "Understanding IMU Noise Density," Daisch Sensor. [Online]. Available: <https://daischsensor.com/understanding-imu-noise-density/> [2] "Inertial Measurement Unit (IMU) - An Introduction," Advanced Navigation. [Online]. Available: <https://www.advancednavigation.com/tech-articles/inertial-measurement-unit-imu-an-introduction/> [3] "Critical Noise Sources MEMS Gyroscopes," Analog Devices. [Online]. Available: <https://www.analog.com/en/resources/technical-articles/critical-noise-sources-mems-gyroscopes.html> [4] "robot_localization," ROS Wiki. [Online]. Available: <https://wiki.ros.org/robot_localization> [5] R. E. Kalman, "A New Approach to Linear Filtering and Prediction Problems," *Transactions of the ASME-Journal of Basic Engineering*, vol. 82, no. 1, pp. 35-45, Mar. 1960. --- *End of Lab 7 Lecture Slides*