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.
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.
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.
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.
5.2.1 MATLAB/Simulink Controller Development¶
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.
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.
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.
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:
5.4 Course-Related Videos¶
Public Lecture Replay (Session 4: Filtering Estimation and Low-Level Control):