This lecture is about UAV filters, including an introduction to UAV filters, measurement principles, linear complementary filter design, linear complementary filter parameter analysis, Kalman filter design, etc.
Filter Design Experiment 2
The Kalman filter is a recursive linear minimum variance estimation algorithm, and its optimal estimate needs to meet the following three conditions:
1) Unbiasedness: that is, the expectation of the estimated value is equal to the true value of the state;
2) The estimated variance is the smallest;
3) Real-time.
Assume that the linear discrete system model is as follows:
In the formula, the statistical characteristics of process noise wk-1 and observation noise vk are:
The Kalman filter algorithm can be summarized as follows:
The Kalman filter also generates an error covariance matrix while performing filter estimation, which can be used to characterize the estimation accuracy and can also be used for sensor health assessment. Generally speaking, if the sampling period is reasonable, the continuous system can be considerable, and the discretized system can also be considerable. However, sometimes the sampling period is improperly selected, and the system may lose controllability and observability. Therefore in principle one should check the observability of discretized systems. HkPk|k-1HTk+Pk|k-1< /span>+PkTHk |k-1PkH(k THk|k-1P= kk needs to be H non-singular, otherwise)-1 cannot be realized. If (ФK,K-1,Hk) is not considerable , then the Kalman filter can still run, but the unobservable modes are not corrected, just recursive. Extreme case Hk=0m`n , then
The implementation of Kalman filter can be found in the file "Attitude_ekf.m", its main parts are as follows
function [ x_aposteriori, P_aposteriori, roll, pitch] =
Attitude_ekf(dt, z, q, r, x_posterior_k, P_posterior_k)
%Function description:
% Extended Kalman filter method for state estimation
%Input:
% dt: update cycle
% z: measured value
% q: system noise, r: measurement noise
% x_aposteriori_k: state estimate at the previous moment
% P_aposteriori_k: Estimated covariance at the last moment
%output:
% x_aposteriori: state estimate at the current moment
% P_aposteriori: estimated covariance at the current moment
% roll, pitch: Euler angle, unit: rad
w_m = z(1:3); % angular velocity measurement value
a_m = z(4:6); % acceleration measurement value
g = norm(a_m,2); %gravitational acceleration
% w_x_=[ 0,-(wz-bzg, wy-byg;
% wz-bzg, 0 ,-(wx-bxg);
% -(wy-byg), wx-bxg, 0];
w_x_ = [0, -(w_m(3) - x_aposteriori_k(3)), w_m(2) -x_aposteriori_k(2);
w_m(3) - x_aposteriori_k(3), 0, -(w_m(1) - x_aposteriori_k(1));
-(w_m(2) - x_aposteriori_k(2)), w_m(1) - x_aposteriori_k(1), 0];
bCn = eye(3, 3) - w_x_*dt;
% predict
% Update a priori state matrix
x_apriori = zeros(1, 6);
x_apriori(1: 3) = x_aposteriori_k(1: 3); %angular velocity drift
x_apriori(4: 6) = bCn*x_aposteriori_k(4: 6); % acceleration normalized value
%[x]x
x_posterior_k_x = [0, -x_posterior_k(6), x_posterior_k(5);
x_posterior_k(6), 0, -x_posterior_k(4);
-x_posterior_k(5), x_posterior_k(4), 0];
% Update state transition matrix
PHI = [eye(3, 3), zeros(3, 3);
-x_posterior_k_x*dt, bCn];
GAMMA = [eye(3, 3)*dt, zeros(3, 3); % noise driver matrix
zeros(3, 3), -x_posterior_k_x*dt];
Q = [eye(3, 3)*q(1), zeros(3, 3);
zeros(3, 3), eye(3, 3)*q(2)];
% Forecast error covariance matrix
P_prior = PHI*P_posterior_k*PHI' + GAMMA*Q*GAMMA';
% renew
R = eye(3, 3)*r(1);
H_k = [zeros(3, 3), -g*eye(3, 3)];
%Kalman gain
K_k = (P_apriori*H_k')/(H_k*P_apriori*H_k' + R);
% state estimation matrix
x_aposteriori = x_apriori' + K_k*(a_m - H_k*x_apriori');
% Estimated error covariance
P_posteriori = (eye(6, 6) - K_k*H_k)*P_a priori;
% Calculate the roll angle and pitch angle, corresponding to roll and pitch respectively.
k = x_posterior(4 : 6) /norm(x_posterior(4 : 6), 2);
roll = atan2(k(2), k(3)); % roll angle
pitch = -asin(k(1)); % pitch angle
end
The filtering results and comparison chart are shown in the figure below. It can be seen that during flight, the Kalman filter has a better effect than the linear complementary filter, and is close to the effect of the PX4's own filter algorithm.
Note: The address of the demo file corresponding to this experiment for versions below RflySim v3.0 is: *\PX4PSP\RflySimAPIs\Exp02_FlightControl\e4-FilterDesign\e4.3;
For RflySim v3.0 and above, the address is: *\PX4PSP\RflySimAPIs\5.RflySimFlyCtrl\1.BasicExps\e4-FilterDesign\e4.3.
references:
[1] Translated by Quan Quan, Du Guangxun, Zhao Zhiyao, Dai Xunhua, Ren Jinrui, and Deng Heng. Multi-rotor aircraft design and control [M], Electronic Industry Press, 2018.
[2] Quan Quan, Dai Xunhua, Wang Shuai. Design and control practice of multi-rotor aircraft [M], Electronic Industry Press, 2020.