卡尔曼滤波(Kalman Filter)应用实例之飞控(Pixhawk)
引言在无人机捷联导航系统中，由加速度计、角速度计、气压计、GPS、磁力计等组成了一个复杂的惯性捷联导航系统，以保证无人机的正常稳定飞行。由于涉及机体坐标系到导航坐标系的变换，以及状态间的耦合关系，导致无人机飞控捷联导航系统构成了一个复杂的非线性动态系统，以速度估计为例，速度更新方程就是一个非线性状态方程。而利用Kalman滤波进行状态最优估计的一个重要前提便是应用对象为线性系统，在最优估计时必然会涉及到线性化的处理。
T_{b}^{n}=\begin{bmatrix} q_{0}^2 + q_{1}^2 - q_{2}^2 - q_{3}^2 & 2*(q_{1}*q_2 - q_{0}*q_3) & 2*(q_1*q_3 + q_0*q_2)\\2*(q_1*q_2 + q_0*q_3) & q_0^2 - q_1^2 + q_2^2 - q_3^2 & 2*(q_2*q_3 - q_0*q_1) \\2*(q_1*q_3-q_0*q_2) & 2*(q_2*q_3 + q_0*q_1) & q_0^2 - q_1^2 - q_2^2 + q_3^2 \end{bmatrix}
% define the velocity update equations
% ignore coriolis terms for linearisation purposes
vNew = + *dt + Tbn*dVelTruth;在进行EKF的飞控系统之前，可在PX4官网下载其导航代码。其下载地址如下，若有兴趣的话，也可把其他代码全部下载到本地。
PX4/ecl
一、捷联导航系统线性化
1.1 状态模型
状态预测方程：
stateVectorNew = F*stateVector + Q
观测方程：
Y=H*stateVector + R
——————————————————
stateVectorNew\Rightarrow 24个状态更新向量
stateVector\Rightarrow 24个状态向量
F\Rightarrow 状态转移矩阵 H\Rightarrow 观测方程
Q\Rightarrow 过程噪声 R\Rightarrow 测量噪声
——————————————————
其中， stateVectorNew 与 stateVector 的定义如下所示：
% Define the state vector & number of states
stateVector = ;
% Define vector of process equations
stateVectorNew = ;1.2 导航系统模型的线性化
在导航工程(\EKF_replay\Filter)中，下面的文件给出了状态转移方程 F 及各观测方程 H 的数学模型，单纯地看这些方程，十分复杂，那怎么计算出来呢？
状态转移矩阵文件:\EKF_replay\Filter\calcF24.m
通过\ecl\EKF\matlab\EKF_replay\Filter\GenerateEquations24.m文件可知，该文件构建了飞控导航系统的模型，利用各状态间的数学模型以及前篇的《涅索斯衬衫：扩展卡尔曼滤波(EKF)之非线性系统的线性化》方法，即可求解上述各捷联导航系统的线性化模型。其部分源码如下所示：
%% define the state prediction equations
% define the measured Delta angle and delta velocity vectors
dAngMeas = ;
dVelMeas = ;
% define the IMU bias errors and scale factor
dAngBias = ;
dVelBias = ;
% define the quaternion rotation vector for the state estimate
quat = ;
% derive the truth body to nav direction cosine matrix
Tbn = Quat2Tbn(quat);
% define the truth delta angle
% ignore coning compensation as these effects are negligible in terms of
% covariance growth for our application and grade of sensor
dAngTruth = dAngMeas - dAngBias;
% Define the truth delta velocity -ignore sculling and transport rate
% corrections as these negligible are in terms of covariance growth for our
% application and grade of sensor
dVelTruth = dVelMeas - dVelBias;
% define the attitude update equations
% use a first order expansion of rotation to calculate the quaternion increment
% acceptable for propagation of covariances
deltaQuat = [1;
0.5*dAngTruth(1);
0.5*dAngTruth(2);
0.5*dAngTruth(3);
];
quatNew = QuatMult(quat,deltaQuat);
% define the velocity update equations
% ignore coriolis terms for linearisation purposes
vNew = + *dt + Tbn*dVelTruth;
% define the position update equations
pNew = + *dt;
% define the IMU error update equations
dAngBiasNew = dAngBias;
dVelBiasNew = dVelBias;
% define the wind velocity update equations
vwnNew = vwn;
vweNew = vwe;
% define the earth magnetic field update equations
magNnew = magN;
magEnew = magE;
magDnew = magD;
% define the body magnetic field update equations
magXnew = magX;
magYnew = magY;
magZnew = magZ;
% Define the state vector & number of states
stateVector = ;
nStates=numel(stateVector);
% Define vector of process equations
stateVectorNew = ;
%% derive the state transition and state error matrix
% Define the control (disturbance) vector. Error growth in the inertial
% solution is assumed to be driven by 'noise' in the delta angles and
% velocities, after bias effects have been removed. This is OK becasue we
% have sensor bias accounted for in the state equations.
distVector = ;
% derive the control(disturbance) influence matrix
G = jacobian(stateVectorNew, );
% derive the state error matrix
distMatrix = diag(distVector);
Q = G*distMatrix*transpose(G);
f = matlabFunction(Q,'file','calcQ24.m');
% derive the state transition matrix
F = jacobian(stateVectorNew, stateVector);
f = matlabFunction(F,'file','calcF24.m');
%% derive equations for fusion of magnetometer measurements
% rotate earth field into body axes
magMeas = transpose(Tbn)* + ;
magMeasX = magMeas(1);
H_MAGX = jacobian(magMeasX,stateVector); % measurement Jacobian
f = matlabFunction(H_MAGX,'file','calcH_MAGX.m');
magMeasY = magMeas(2);
H_MAGY = jacobian(magMeasY,stateVector); % measurement Jacobian
f = matlabFunction(H_MAGY,'file','calcH_MAGY.m');
magMeasZ = magMeas(3);
H_MAGZ = jacobian(magMeasZ,stateVector); % measurement Jacobian
f = matlabFunction(H_MAGZ,'file','calcH_MAGZ.m');
%% derive equations for fusion of synthetic deviation measurement
% used to keep correct heading when operating without absolute position or
% velocity measurements - eg when using optical flow
% rotate magnetic field into earth axes
magMeasNED = ;
% the predicted measurement is the angle wrt magnetic north of the horizontal
% component of the measured field
angMeas = atan(magMeasNED(2)/magMeasNED(1));
H_MAGD = jacobian(angMeas,stateVector); % measurement Jacobian
H_MAGD = simplify(H_MAGD);
f = matlabFunction(H_MAGD,'file','calcH_MAGD.m');
%% derive equations for fusion of a single magneic compassheading measurement
% rotate body measured field into earth axes
magMeasNED = Tbn*;
% the predicted measurement is the angle wrt true north of the horizontal
% component of the measured field
angMeas = atan(magMeasNED(2)/magMeasNED(1));
H_MAG = jacobian(angMeas,stateVector); % measurement Jacobian
f = matlabFunction(H_MAG,'file','calcH_HDG.m');通过该文件计算相应的线性模型后，根据当前的标称轨迹，求解出领域内的线性化模型。利用卡尔曼滤波方法，即可进行状态的最优估计。如磁力计融合为例（\ecl\EKF\matlab\EKF_replay\Filter\FuseMagnetometer.m）。
% calculate the observation jacobian, innovation variance and innovation
for obsIndex = 1:3
% Calculate corrections using X component
if (obsIndex == 1)
H(1,:) = calcH_MAGX(magD,magE,magN,q0,q1,q2,q3);
elseif (obsIndex == 2)
H(2,:) = calcH_MAGY(magD,magE,magN,q0,q1,q2,q3);
elseif (obsIndex == 3)
H(3,:) = calcH_MAGZ(magD,magE,magN,q0,q1,q2,q3);
end
varInnov(obsIndex) = (H(obsIndex,:)*P*transpose(H(obsIndex,:)) + R_MAG);
innovation(obsIndex) = magPred(obsIndex) - magMea(obsIndex);
end二、飞控导航系统仿真
[*]数据的导入
按照\EKF_replay\readme.txt的方式对实飞数据进行格式转化，并存放在相应的目录内。当然，若没有实飞数据，则可直接下载官网的飞行测试数据进行算法的仿真测试即可。
[*]EKF导航系统的仿真测试
[*]将\ecl\EKF\matlab\EKF_replay\Filter设为当前路径。
[*]根据实际的飞控条件，配置相应的系统参数.\EKF_replay\SetParameters.m。
[*]根据实际的测试数据类型（PX4或APM的数据），运行.\EKF_replay\replay_apm_data.m文件或.\EKF_replay\replay_px4_data.m文件。
[*]当运行完成后，文件夹OutputPlots即可查看导航测试结果。如下所示：
三、飞控导航系统的文件转化
起初，若直接看EKF飞控源码的话，可能会被其中的矩阵式的表示方式直接吓到。如估计误差协方差矩阵的求取(\ecl\EKF\covariance.cpp)，实际上状态转移矩阵、观测矩阵以及协方差矩阵都是由matlab文件直接转化而来。详情可查看如下文件：
\ecl\EKF\matlab\scripts\Inertial Nav EKF\GenerateNavFilterEquations.m
参考文献
Robot Localization and Kalman Filters
PX4: ROS with Gazebo Simulation
<hr/>
页:
[1]