[Hybrid Navigation System Series Part 5: Implementation-Stage 1] Cruising Phase: Classical Dead Reckoning, Wind Estimation, and Gazebo Wind Simulation

Bymaponarooo

Hello to all the graduate students and researchers burning the midnight oil at the forefront of autonomous unmanned aerial vehicle (UAV) systems! Welcome back to QUAD Drone Lab.

In our previous post, <Part 4>, we successfully completed the infrastructure work by building a ROS 2 & Micro XRCE-DDS bridge to inject high-performance external INS data into the PX4 EKF2 filter. Now, our kamikaze drone is equipped with a robust “neural network” that allows it to estimate its own position using external sensors even in a GPS-denied environment.

Starting today, we finally dive into the [Implementation] phase. The theme of <Part 5> is the ‘Cruising Phase’. For a kamikaze drone flying hundreds of kilometers, what is the greatest enemy it faces in the sky? The answer is ‘Wind’.

When GPS is available, the drone constantly corrects its absolute position even if pushed by the wind, smoothly maintaining its path. However, when flying purely on Dead Reckoning in a GNSS-Denied environment, encountering a strong crosswind can sweep the drone several kilometers off course.

In this post, we will resurrect the ‘Crab Angle’ calculation—a survival technique used by World War II pilots—and apply it to modern robotics. We will deeply explore the real-time wind estimation principles of PX4’s EKF2, write a ROS 2 Python node to correct the heading, and validate our logic using a Wind Injection test in the Gazebo simulator.

Let’s get started!

1. Classical Dead Reckoning and the Revival of the ‘Crab Angle’

During World War II, fighter pilots flying over the vast Pacific Ocean without GPS knew that ‘wind’ was a matter of life and death. If a target is located due north (0 degrees) and the pilot simply points the nose of the aircraft due north, a crosswind blowing from the west will eventually push the aircraft far to the east.

To overcome this, pilots slightly turned the nose of the plane into the wind, essentially flying somewhat sideways (like a crab) to maintain a straight ground track. This technique is known as flying with a Crab Angle or Wind Correction Angle (WCA).

Mathematically, the Wind Correction Angle (WCA) is calculated using the following formula: WCA=arcsin(Va​Vw​sin(θ)​)

  • Vw​ (Wind Speed): The speed of the wind.
  • θ (Wind Angle): The angle between the desired course and the direction the wind is blowing from.
  • Va​ (True Airspeed): The true airspeed of the aircraft.

This simple vector synthesis formula is the absolute lifeline for dead reckoning over hundreds of kilometers without GPS. The question then becomes: How can our drone know the wind speed (Vw​) and wind direction (θ) in real-time?

2. Real-Time Wind Estimation in PX4 EKF2

Fortunately, the brain of our drone, the EKF2 (Extended Kalman Filter) in PX4, has a highly sophisticated built-in mechanism for estimating wind speed and direction.

✈️ Wind Estimation in Fixed-Wing UAVs

Fixed-wing drones directly measure their airspeed (Va​) using a Pitot tube mounted on the front. The EKF2 filter calculates the wind vector by taking the difference between the Ground Speed (provided by GPS or our external INS) and the Airspeed vector. Furthermore, even when GPS is denied, PX4 utilizes a Synthetic Sideslip assumption (EKF2_FUSE_BETA = 1), assuming that the lateral slip is zero, to effectively estimate the wind.

🚁 Wind Estimation in Multicopters: Drag Specific Forces

What about multicopters, which typically do not carry Pitot tubes? When a multicopter hovers or flies forward against the wind, it must tilt slightly into the wind (introducing Pitch/Roll) to overcome aerodynamic drag. The PX4 EKF2 analyzes this Specific Force measured by the IMU’s accelerometers and works backward to estimate the current North and East wind velocities.

Researcher Tuning Tip: To activate this feature on a multicopter, you must configure the following parameters in QGroundControl:

  • EKF2_DRAG_CTRL: Enable drag fusion.
  • EKF2_BCOEF_X, EKF2_BCOEF_Y: The ballistic coefficients in the X and Y axes. These must be tuned based on the drone’s shape, frontal area, and weight.
  • Pro Tip: Fly the drone in Position mode on a windless day, performing aggressive stop-and-go maneuvers in all directions. Download the .ulg log file and run the mc_wind_estimator_tuning.py script provided by PX4 to automatically calculate the optimal ballistic coefficients!

3. [Hands-on] Implementing Crab Angle Correction in ROS 2 Python

Now, let’s write the control node that will run on our Companion Computer via ROS 2. This code subscribes to the real-time wind estimates calculated by PX4 (WindEstimate message), calculates the required Crab Angle (WCA), and adjusts the drone’s Yaw to command a corrected TrajectorySetpoint.

Python
import rclpy
from rclpy.node import Node
import math

# Import PX4 uORB messages
from px4_msgs.msg import WindEstimate, TrajectorySetpoint, VehicleOdometry

class DeadReckoningWindCorrectionNode(Node):
    def __init__(self):
        super().__init__('dr_wind_correction_node')

        # Subscribe to wind estimates (Real-time wind speed/direction from EKF2)
        self.wind_sub = self.create_subscription(
            WindEstimate,
            '/fmu/out/wind_estimate',
            self.wind_callback,
            10
        )
        
        # Subscribe to vehicle odometry (To estimate current airspeed)
        self.odom_sub = self.create_subscription(
            VehicleOdometry,
            '/fmu/out/vehicle_odometry',
            self.odom_callback,
            10
        )

        # Publisher for Offboard control setpoints
        self.setpoint_pub = self.create_publisher(
            TrajectorySetpoint,
            '/fmu/in/trajectory_setpoint',
            10
        )

        # Control loop timer running at 10Hz
        self.timer = self.create_timer(0.1, self.control_loop)

        # Initialize state variables
        self.wind_north = 0.0
        self.wind_east = 0.0
        self.current_vel_x = 0.0
        self.current_vel_y = 0.0

        # Define desired course (e.g., fly due North at 0 degrees)
        self.desired_course_deg = 0.0  
        self.target_speed = 10.0 # m/s

    def wind_callback(self, msg):
        # Extract North and East wind velocities from EKF2
        self.wind_north = msg.windspeed_north
        self.wind_east = msg.windspeed_east

    def odom_callback(self, msg):
        # Extract current ground velocities
        self.current_vel_x = msg.velocity
        self.current_vel_y = msg.velocity[1]

    def control_loop(self):
        # 1. Calculate absolute Wind Speed and Wind Direction (radians)
        wind_speed = math.sqrt(self.wind_north**2 + self.wind_east**2)
        wind_dir_rad = math.atan2(self.wind_east, self.wind_north)
        
        # 2. Convert Desired Course to radians
        desired_course_rad = math.radians(self.desired_course_deg)
        
        # 3. Calculate Wind Angle (Difference between desired course and wind direction)
        wind_angle_rad = wind_dir_rad - desired_course_rad
        
        # 4. Calculate the Crab Angle (Wind Correction Angle, WCA)
        # WCA = arcsin( (WindSpeed * sin(WindAngle)) / TrueAirspeed )
        # Here, we approximate airspeed using our target ground speed
        airspeed = max(self.target_speed, 1.0) # Prevent division by zero
        crosswind_component = wind_speed * math.sin(wind_angle_rad)
        
        # Prevent physical limits (If crosswind > airspeed, drone cannot fly the course)
        if abs(crosswind_component) > airspeed:
            crosswind_component = math.copysign(airspeed * 0.99, crosswind_component)
            self.get_logger().warn("Warning: Crosswind is stronger than the vehicle's airspeed!")

        wca_rad = math.asin(crosswind_component / airspeed)
        
        # 5. Calculate the Required Heading by subtracting WCA from desired course
        required_heading_rad = desired_course_rad - wca_rad

        # 6. Publish the TrajectorySetpoint for PX4 Offboard mode
        sp_msg = TrajectorySetpoint()
        sp_msg.timestamp = int(self.get_clock().now().nanoseconds / 1000)
        
        # In the Global NED frame, command the velocity vector along the desired course,
        # but twist the drone's nose (Yaw) into the wind to compensate.
        sp_msg.velocity = [
            self.target_speed * math.cos(desired_course_rad),
            self.target_speed * math.sin(desired_course_rad),
            0.0 # Maintain current altitude
        ]
        sp_msg.yaw = required_heading_rad

        self.setpoint_pub.publish(sp_msg)

def main(args=None):
    rclpy.init(args=args)
    node = DeadReckoningWindCorrectionNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

When you run this code, you will visually see the drone automatically twist its nose into the wind (crabbing) while maintaining a perfectly straight flight path over the ground!

4. Simulating Crosswind Injection in Gazebo SITL

To verify that our algorithm actually works, we need to artificially blow “fake wind” into our simulation world. In the Gazebo (Ignition or Classic) environment, we can easily achieve this using the windy.sdf.

“Join our QUAD Drone Lab YouTube channel as a [💎Diamond Member] to get full access to all the textbooks and source code for your flight tests.”

XML
    <wind>
      <linear_velocity>0 0 0</linear_velocity>
    </wind>

Interpretation of the Experiment Results:

  1. Before Correction: If commanded to fly due North at 10m/s for 10 minutes without correction, the 10m/s Easterly crosswind completely sweeps the drone away. After 10 minutes, the drone ends up a massive 6 km east of the intended target path. A complete mission failure.
  2. After Crab Angle Correction (Running the Python Node): The drone instantly uses the EKF2 wind estimates to yaw its nose 45 degrees to the West, actively fighting the crosswind. After 10 minutes, it arrives within a mere few dozen meters of the intended target coordinates. An incredible success!

Wrapping Up and Preview for Part 6

In this <Part 5>, we successfully applied the mathematical principles of the ‘Crab Angle’—a legacy of WWII aviation—to modern robotics. Using ROS 2 Python and the Gazebo simulator, we implemented the logic to cruise toward our target without being swept away by the wind, even in a GPS-denied environment.

Our kamikaze drone can now cut through fierce winds and press forward silently. However, a critical problem remains. No matter how expensive our tactical INS is or how perfectly we calculate the wind correction angle, flying 100km to 200km will inevitably result in accumulated drift. Sensor noise double-integration and minor wind estimation errors will eventually compound into a deviation of tens or hundreds of meters (as we simulated back in Part 3!).

To achieve a pinpoint strike, we must completely “Reset” this accumulated error to zero at specific checkpoints during the flight—just as Navy pilots once visually checked the shapes of islands against their maps.

Therefore, in our next post, <Part 6: [Implementation-Stage 2] Mid-course Correction Phase: OpenCV/YOLO Terrain Matching and Digital Twin Testing>, we will delve into the crown jewel of UAV navigation. We will explore Terrain Referenced Navigation (TRN) and Template Matching techniques, utilizing a downward-facing camera to visually recognize landmarks (buildings, islands, etc.) and shave off the accumulated drift. Stay tuned!

YouTube Tutorial

Author: maponarooo, CEO of QUAD Drone Lab

Date: May 11, 2026

Similar Posts

답글 남기기