01 What is Kalman filter?
Kalman filtering is a filtering algorithm that uses state space in the time domain and is recursively obtained, which is convenient for real-time implementation on a computer and has a small amount of calculation and storage. This method can deal with the filtering problem of multivariable non-stationary random process and the filtering problem of time-varying system. For example, during the flight of an aircraft, the interference encountered is usually time-varying non-stationary noise. In this case, the Kalman filter can be used to effectively remove the interference and obtain more realistic state estimation data.
02 What is the role of Kalman filter in the flight control system?
During the flight of an aircraft, the pitch and roll angle and heading angle of the aircraft are usually provided by a magnetometer and a gyroscope. The position and speed of the aircraft are usually obtained by the mechanism modeling of the accelerometer. Simply put, the angle and position speed of the aircraft are data. It can be obtained by sensors and flight control mechanism algorithms, but because the aircraft will have noise input during flight, the state equation and observation equation will have certain noise input at this time, then the feedback data is fuzzy and inaccurate. How to remove these What about external interference noise, so that the aircraft can get real flight data? Then Kalman filtering can achieve such a function.
03 Examples:
As shown in Figure (1), the aircraft has external noise input during the flight. The sensor collects the interfering data and sends it to the filter estimator. The filter is responsible for removing these noises and restoring the aircraft’s real data to the controller. The real data of the aircraft is controlled and output to the aircraft. In this way, the aircraft has real data, and the aircraft can be better controlled to achieve the purpose of stable flight.
- Kalman filter formula
Q and R are the variance matrix of w(t), v(t); P is the variance matrix of state estimation.
Note: The cited formula is from "Modeling and Estimation" p126-p129 by Professor Deng Zili of Heilongjiang University.
-
Kalman filter simplified
Simplify and merge the first three steps of Kalman filter formula (2) to obtain the following formula:
- Kalman filter recursive algorithm block diagram
- Simplified block diagram of Kalman filter recursive algorithm
Implementation of Kalman Filter in Matlab
The following is a preliminary explanation of the implementation of Kalman filtering in Matlab. All the materials used are from the first lesson of "Kalman Filter (Theory)" produced by Amu Lab-Bobay College. Interested readers can watch the video explanation step by step. The lessons are free trial lessons. (Bamboo Academy: https://bbs.amovlab.com/plugin.php?id=zhanmishu_video:video&mod=video&cid=19 )
◆As shown in Figure (2) and Figure (3), taking the position and speed of the trolley as the state estimation target, the physical body relationship between position and speed is used to model the trolley to obtain the state estimation equation; model the sensor of the trolley to obtain Observation equation.
◆Simplify Kalman filter and get the formula shown in Figure (4).
◆Edit the code in matlab. code show as below:
◆Observe state estimation output
The initial state of the position and velocity of the trolley is 0. When there is a noise input with a variance of 1, the position and velocity of the trolley is not 0. At this time, the position and velocity obtained by the Kalman filter estimator is estimated as shown in Figure (5).
When the input variance is 10 noise, the position velocity estimation is shown in Figure (6). Compared with Figure (5), it can be seen that the greater the input noise, the greater the impact on system stability.
The notes on Kalman filtering in this article are all personal understanding, please criticize and correct any errors.
Author: Amu R & D laboratory - Sun voyage
Welcome all teaching assistants to participate in the contribution~