Skip to content

Chapter 5: Filtering Estimation and Low-Level Control

After completing the 3D modeling and motion modeling of the vehicle, the next step is to develop a low-level control system to ensure the vehicle can stably follow predefined commands. For the low-level control system, filtering estimation algorithms are crucial, as they extract precise and smooth state estimates from noisy and erroneous sensor data; low-level control algorithms, based on the filtering estimates and combined with user input commands, drive actuators and power systems to achieve stable vehicle control. This chapter first reviews the relevant background theory of filtering and control, then introduces the design methods, development procedures, and tuning strategies for filters and controllers.

5.1 Background and Theory

In intelligent unmanned systems, the control layer is typically divided into two hierarchical levels: the low-level control system and the high-level control system. The core components of the low-level control system include vehicle filtering and control (covered in this chapter), external control and trajectory planning (detailed in Chapter 6), and health management and safety decision-making (detailed in Chapter 7). Its primary objective is to construct a foundational intelligent agent composed of the vehicle body and control systems, enabling stable and reliable pose and position state perception and control, while receiving decision-making commands from the high-level system via standardized external interfaces.

alt text

5.1.1 Filtering Estimation Principles

In intelligent unmanned systems, filters and controllers constitute the two core modules of the low-level control system, directly impacting system stability, accuracy, and autonomy. Specifically, the controller is responsible for real-time adjustment of the unmanned system's spatial position and attitude, enabling it to follow a predefined trajectory or target; the filter fuses data from multiple sensors to provide accurate state estimation (including pose, velocity, etc.), compensating for noise and errors, thereby delivering reliable state feedback to the controller. These two technologies complement and collaborate with each other, ensuring the unmanned system efficiently and stably executes tasks in complex, dynamic environments.

A typical automatic control system comprises three core components: the plant, the controller, and the feedback loop. The controller computes control commands based on the deviation between the desired value and the actual output; the plant executes corresponding actions according to the control commands; the feedback loop measures the actual state of the plant via sensors and feeds this information back to the controller, forming a closed-loop control system.

5.1.2 Controller Design Fundamentals

In the field of unmanned systems, common control algorithms include PID control, fuzzy control, adaptive control, and sliding mode control. Among them, PID control is widely adopted due to its simple structure and ease of implementation. A PID controller consists of three components—proportional (P), integral (I), and derivative (D)—which collectively regulate the error based on its past, present, and future trends to achieve precise control of the plant.

alt text

For multi-rotor UAVs, a typical control architecture employs a cascaded control scheme: the outer loop handles position control, while the inner loop manages attitude control. The position controller calculates the desired attitude angles based on the target position; the attitude controller then computes motor control commands based on the desired attitude angles, ultimately driving the motors via electronic speed controllers (ESCs) to generate the required thrust and torque.

alt text

5.2 Framework and Interfaces

RflySim adopts the Model-Based Design (MBD) methodology for unmanned system control and safety testing. It implements a complete workflow from design to verification through five stages: modeling, controller design, software-in-the-loop simulation (SIL), hardware-in-the-loop simulation (HIL), and actual flight testing.

alt text

MATLAB/Simulink provides interfaces for multi-rotor controller design, enabling users (including beginners, students, and engineers) to rapidly design and validate controllers using their existing knowledge. The platform offers a rich library of modules covering core functionalities such as sensor data processing, filter design, attitude control, and position control.

alt text

Leveraging MATLAB/Simulink's automatic code generation technology, the controller can be conveniently downloaded to hardware for HIL simulation and actual flight testing. The development process is as follows: first, design and simulate the controller in Simulink; then, use the PX4PSP toolbox to generate C/C++ code; next, compile the generated code into the PX4 autopilot firmware; finally, automatically deploy it to the flight controller hardware using the download functionality provided by the toolchain.

5.2.2 Code Generation and Firmware Deployment

After completing the controller design, the RflySim toolchain provides code generation and download functionality, enabling the generated Simulink control algorithm to be converted into C/C++ code, compiled into the PX4 autopilot firmware, and automatically downloaded to the autopilot. The toolchain also supports hardware-in-the-loop simulation testing, allowing users to conduct preliminary flight performance simulations on real Pixhawk autopilot hardware, thereby identifying and resolving potential issues before actual flight tests.

alt text

After successful testing, the Pixhawk autopilot can be mounted onto the multi-rotor hardware system to conduct indoor and outdoor flight experiments, evaluating the performance of the designed control algorithm through experimental validation.

5.2.3 Sensor Interface and Data Acquisition

For sensor calibration and filter design, acquisition of raw sensor data is required. RflySim provides a complete sensor interface. For controller operation, filtered attitude and position information, along with remote controller input commands, are needed to generate motor control laws. The toolchain supports real-time acquisition of autopilot internal state data—including raw sensor data, filtered state estimates, and control commands—via the MAVLink protocol.

alt text

5.3 Showcase of Representative Cases

Autopilot Low-Level Control Development Series Course:

Feisi X150 Aircraft Bench Experiment:

Degraded Control for Quadcopter with Three-Motor Failure:

Public Lecture Replay (Session 4: Filtering Estimation and Low-Level Control):

5.5 Experimental Cases in This Chapter

The relevant verification experiments and guided cases for this chapter are stored in the [Installation Directory]\RflySimAPIs\5.RflySimFlyCtrl folder.

5.5.1 Interface Learning Experiments

Stored in the 5.RflySimFlyCtrl\0.ApiExps folder, covering platform basic interface guidance and general tool introduction.

Experiment 1: Installation of Low-Level Flight Controller Development Resource Files

📝 Experiment Overview: Introduces driver software and learning resources required for RflySim experiments, including installation and configuration of the 3DR-X 6-channel telemetry driver and standard documentation for Pixhawk 6X/6C flight controller hardware.

Experiment 2: Automatic Code Generation for SITL Full-Throttle Motor Control

📝 Experiment Overview: Learn automatic code generation using Simulink TLC language, converting controller models into PX4 flight controller code, and verifying the correctness of the generated full-throttle motor control code in the RflySim SITL simulation environment.

Experiment 3: Software-in-the-Loop Simulation Experiment

📝 Experiment Overview: Software-in-the-loop simulation experiment based on the Simulink/RflySim3D platform. Learn to use a Simulink controller to arm and remotely control a quadcopter, gaining familiarity with core modules such as controller subsystems, attitude control, and motor allocation.

Experiment 4: Real-Time Parameter Adjustment via QGC

📝 Experiment Overview:
Observe flight status in real time using QGroundControl (QGC) and tune Pixhawk flight controller parameters SL_RFLY_FLT and SL_RFLY_INT to achieve optimal control performance.

Experiment 5: Flight Controller State Data Acquisition

📝 Experiment Overview:
Acquire key vehicle state data—including attitude, battery status, sensor fusion, GPS, and IMU data—via RflySim's low-level development interface. Learn to monitor flight controller status using uORB message reading interfaces.

Experiment 6: Autopilot CPU Usage Monitoring

📝 Experiment Overview:
This experiment demonstrates how to monitor the CPU usage of the PX4 autopilot system via the Mavlink console in QGroundControl. Learn to use the top command and px4_simulink_app-related commands to monitor flight controller load, avoiding experiment failure due to CPU overload caused by overly complex algorithms.

Experiment 7: Resource Usage Comparison of Simulink Function Blocks

📝 Experiment Overview:
Compare resource usage of M-Function and S-Function blocks in Simulink on flight controller hardware. By running an accumulation algorithm on the Pixhawk6C, this experiment demonstrates that S-Function consumes less CPU and memory resources than M-Function.

Experiment 8: RC Receiver Signal Processing Module

📝 Experiment Overview:
Validate RC receiver signal processing functionality, including the InputRcCali calibration module, InputRcNorm normalization module, and RCOverCtrlAPI manual control override feature, ensuring accurate and reliable flight control via the remote controller.

Experiment 9: Actuator Control Signal API Experiment

📝 Experiment Overview:
Drive the inPWMs input of CopterSim by sending actuator_outputs_rfly messages, enabling PWM control, normalized control, 4D/6D torque-thrust control, and PWM/AUX output testing. This validates the control performance of Simulink-based controllers in actual flight scenarios.

Experiment 10: Offboard Control API Validation Experiment

📝 Experiment Overview:
Learn various control modes in Offboard mode—including position, velocity, acceleration, Euler angle, and attitude rate control—by utilizing interfaces such as OffboardAdvCtrlAPI and OffboardAttCtrlAPI to validate external computer-based drone control.

Experiment 11: Mavros Communication API Simulation Control

📝 Experiment Overview:
Learn to use the Mavros communication interface, including hardware-in-the-loop (HIL), software-in-the-loop (SIL) simulations, and C++ testing experiments in both ROS 1 and ROS 2 environments.

Experiment 12: VTOL PWM Output Hardware-in-the-Loop Testing

📝 Experiment Overview:
Based on the Pixhawk 6X flight controller running PX4 firmware v1.15.0, this experiment achieves real PWM output during hardware-in-the-loop simulation, synchronously controlling actual motors and servos to validate vertical-takeoff-and-landing (VTOL) aircraft experiments in a hybrid virtual-physical setup.

Experiment 13: Automatic Code Generation for Attitude Controller

📝 Experiment Overview: Learn to use the Simulink TLC mechanism for automatic code generation of attitude controllers, and verify the generated flight control code in the RflySim platform's SITL simulation environment.

Experiment 14: PSP Toolbox Hardware-in-the-Loop Simulation

📝 Experiment Overview: Familiarize yourself with experiment resources provided by PSP. Through the px4demo_input_rc.slx experiment, understand the hardware-in-the-loop simulation process and master the method of accessing internal flight controller parameters via the PSP toolbox for tuning and testing.

Experiment 15: Real Flight Output Signal Verification for Quadcopter UAV

📝 Experiment Overview: This experiment helps users troubleshoot the issue of motors not spinning after arming a quadcopter UAV. It verifies PWM and AUX output interfaces based on the Pixhawk 6X flight controller, teaching the difference between I/O and FMU, as well as motor drive debugging methods.

Experiment 16: RePX4Block Controller Switching Experiment

📝 Experiment Overview: This experiment uses the RePX4Block module and an RC auxiliary channel (ch8) to achieve real-time switching between the PX4 official controller and a Simulink custom controller, and verifies the switching prompt logs in QGC.

Experiment 17: Attitude Controller SIL/HIL/Real Flight Experiment

📝 Experiment Overview: Through a complete attitude controller development process, master PID controller design, Simulink software-in-the-loop simulation, hardware-in-the-loop simulation, and real flight verification techniques, completing the full workflow of multi-rotor UAV attitude control.

Experiment 18: Offboard Forwarding Control Automatic Code Generation

📝 Experiment Overview: Learn to use Simulink TLC language for automatic code generation, and master the method of verifying automatically generated Pixhawk flight controller code in the RflySim platform's SITL simulation environment.

Experiment 19: Flashing Flight Controller Firmware via QGC

📝 Experiment Overview: Flash PX4 flight controller firmware using QGC software, familiarize yourself with methods for restoring flight controller firmware, and master firmware version selection and compatibility knowledge.

Experiment 20: Binary Log Recording Module Experiment

📝 Experiment Overview: Use the binary_logger module to write and read flight data in .bin and .ulg formats, familiarizing yourself with the underlying operational logic of the PX4 flight controller.

Experiment 21: Custom uORB Message Read/Write

📝 Experiment Overview: Implement read/write functionality by creating custom uORB messages, mastering data interaction capabilities between internal modules of the PX4 uORB message system. Includes experiments on message reading, function-triggered reading, message sending, and handling ID and filename inconsistencies.

Experiment 22: Custom uORB Message Creation

📝 Experiment Overview: Implement read/write functionality by creating custom uORB messages, familiarizing yourself with and mastering the PX4 uORB message system, including message definition, subscribing to actuator_armed, and publishing rfly_px4 messages.

Experiment 23: Mavlink Message Echo Experiment

📝 Experiment Overview: Echo the RC receiver status externally via the mavlink_log uORB message, displaying the system's operational status in real time.

Experiment 24: PX4 External Control Parameter Tuning

📝 Experiment Overview: Use externally sent rfly_ctrl messages as remote controller input to achieve PX4 flight controller hardware-in-the-loop simulation. Learn external communication interface configuration, switching between three control modes, and data interaction methods between Simulink/Python and the flight controller.

Experiment 25: ROS1 Hardware-in-the-Loop Simulation Experiment

📝 Experiment Overview: Conduct hardware-in-the-loop simulation experiments in a ROS1 environment. Communicate with the PX4 flight controller via Msg2SimulinkRosAPI.py, observe MAVLink information exchange, and master the communication method between ROS1 and the flight controller.

Experiment 26: ROS1 Software-in-the-Loop Simulation

📝 Experiment Overview: This experiment primarily focuses on learning software-in-the-loop simulation in a ROS1 environment. Use Msg2SimulinkRosAPI.py and the PX4MavCtrlV4ROS library to achieve data interaction with the Pixhawk flight controller, and observe the ROS1 information exchange mechanism.

Experiment 27: ROS1 C++ Communication Test

📝 Experiment Overview: Observe ROS 1 data exchange via a C++ program in a WSL Ubuntu environment, learning the communication principles and methods between the PX4 flight controller system and the ROS interface.

Experiment 28: ROS2 Hardware-in-the-Loop Simulation Experiment

📝 Experiment Overview: Conduct hardware-in-the-loop simulation experiments in a ROS2 environment. Communicate with the PX4 flight controller via Msg2SimulinkRosAPI.py, observe ROS2 information exchange, and learn ROS2 communication interfaces and hardware-in-the-loop testing methods.

Experiment 29: SITL Simulation Experiment in ROS2 Environment

📝 Experiment Overview: Conduct software-in-the-loop simulation experiments in a ROS 2 environment. Learn to use the PX4MavCtrlV4ROS library for communication and control with the PX4 flight controller, mastering the method of data exchange between ROS 2 and the flight controller.

Experiment 30: C++ Test Experiment in ROS2 Environment

📝 Experiment Overview: In a WSL Ubuntu environment, use a C++ program to perform MAVLink communication with the PX4 flight controller via ROS 2, achieving data exchange. Includes SITL and HITL simulation modes. Learn MAVROS interface development, and master the connection configuration and troubleshooting methods between ROS 2 nodes and the flight controller.

Experiment 31: RflySim Multi-Rotor Frame Type Configuration

📝 Experiment Overview: Select different multi-rotor frame types such as Tricopter, Quadrotor, Hexarotor, Octorotor for simulation by modifying the ModelParam_uavType parameter in Init.m.

Experiment 32: RflySim Multi-Rotor Frame Type Configuration

📝 Experiment Overview: Select different multi-rotor frame types such as Tricopter, Quadrotor, Hexarotor, Octorotor for simulation by modifying the ModelParam_uavType parameter in Init.m.

Experiment 33: RflySim Multi-Rotor Frame Type Configuration

📝 Experiment Overview: Select different multi-rotor frame types such as Tricopter, Quadrotor, Hexarotor, Octorotor for simulation by modifying the ModelParam_uavType parameter in Init.m.

Experiment 34: RflySim Multi-Rotor Frame Type Configuration

📝 Experiment Overview: Select different multi-rotor frame types such as Tricopter, Quadrotor, Hexarotor, Octorotor for simulation by modifying the ModelParam_uavType parameter in Init.m.

Experiment 35: RflySim Multi-Rotor Frame Type Configuration

📝 Experiment Overview: Select different multi-rotor frame types such as Tricopter, Quadrotor, Hexarotor, Octorotor for simulation by modifying the ModelParam_uavType parameter in Init.m.

5.5.2 Basic Usage Experiments

Stored in the 5.RflySimFlyCtrl\1.BasicExps folder, providing a complete set of supplementary teaching materials for beginners.