【PX4 EKF simulink仿真程序解析】（一）初始化

PX4 EKF simulink仿真程序解析（一）——初始化

整体框架如下：

进入InertialNavFliter，整体框架如下：

初始化过程包括协方差初始化、状态向量初始化。其中包括测量值的处理和地球自转角速度的处理。
先来看协方差初始化及地球自转角速度的处理：

这个部分的输入只有飞机所处的纬度数据，输出初始化后的P和地球自转向量，详细如下：

function [earthRateNED, covariance] = calculateInitialData(latDeg)
%#codegen
% 转弧度角系数
deg2rad = single(pi/180);

% Define the initial Euler angle covariance (Phi, Theta, Psi)
%角度协方差初始化，航向角要大几个数量级
InitialEulerCovariance  = single([(1.0*pi/180); (1.0*pi/180); (10.0*pi/180)].^2);

% Define the transformation vector from a 321 sequence Euler rotation vector to a q0,...,q3 quaternion vector
% linearised around a level orientation (roll,pitch = 0,0)
%按照321的顺序旋转，转换四元数矩阵，
%因为欧拉转四元数是非线性的，所以用到了雅可比矩阵求偏导Jaccobian（Q，Euler）
% Q = 【1,0.5*roll,0.5*pitch,0.5*yaw】，这里的角是极小值
J_eul2quat = ...
    single([[ 0.0, 0.0, 0.0]; ...
    [ 0.5, 0.0, 0.0]; ...
    [ 0.0, 0.5, 0.0]; ...
    [ 0.0, 0.0, 0.5]]);

% Form the covariance matrix for the intial Euler angle coordinates
%对角化矩阵
angleCov = diag(InitialEulerCovariance);

% Transform the Euler angle covariances into the equivalent quaternion covariances
%四元数协方差，
quatCov = J_eul2quat*angleCov*transpose(J_eul2quat);

% define the state covariances with the exception of the quaternion covariances
%初始化赋值，可以看到这里位置的初始化误差很大
Sigma_vel_NE = single(0.7); % 1 sigma uncertainty in horizontal velocity components
Sigma_vel_D  = single(0.7); % 1 sigma uncertainty in vertical velocity
Sigma_pos_NE = single(15); % 1 sigma uncertainty in horizontal position components
Sigma_pos_D  = single(5.0); % 1 sigma uncertainty in vertical position
Sigma_dAng   = single(1/10*pi/180*0.02); % 1 Sigma uncertainty in delta angle bias
Sigma_dVel   = single(0.1*0.02); % 1 Sigma uncertainty in delta velocity bias
Sigma_wind   = single(8);
Sigma_MagNED = single(20);
Sigma_MagXYZ = single(20);
covariance   = single(diag([0;0;0;0;Sigma_vel_NE*[1;1];Sigma_vel_D;Sigma_pos_NE*[1;1];Sigma_pos_D;Sigma_dAng*[1;1;1];Sigma_dVel*[1;1;1];Sigma_wind*[1;1];Sigma_MagNED*[1;1;1];Sigma_MagXYZ*[1;1;1]].^2));

% Add the quaternion covariances
%完成协方差初始化
covariance(1:4,1:4) = quatCov;

%Define Earth rotation vector in the NED navigation frame
%地球旋转矢量，7.2921这个是地球自转角速度rad/s
%latDeg是维度
earthRateECEF = single([0; 0; 7.2921e-005]);
%简单地将真北向量分解到ND上
earthRateNED  = ...
    single([cos(latDeg*deg2rad)*earthRateECEF(3); ...
    0; ...
    -sin(latDeg*deg2rad)*earthRateECEF(3)]);

Euler角度怎么获取的还不清楚，四元数由AHRS的欧拉角转换得到，速度和位置由测量值得到，偏置和风速初始化为0，NEDMag来源于磁力计数据Tbn - Mag偏置得到，但是MagXYZ（也就是偏置）暂时不知道怎么得到的。

状态值储存模块：Store States储存了50个时间步的数据，用一个2450的矩阵保存，按1-50列的顺序循环刷新数据，同时记录了对应的时间戳，代码如下：

%#codegen
function [statesStoreOut,msecStoreOut]   = StoreStates(states,time)
% 记录了50条状态值，和对应的时间。  % 0.05s

persistent statesStore;
if isempty(statesStore)
    statesStore = single(zeros(24,50));
end

persistent index;
if isempty(index)
    index = 1;
end

persistent msecStore;
if isempty(msecStore)
    msecStore = uint32(zeros(1,50));
end

if (index > 50)
    index = 1;
end
for i=1:24
    statesStore(i,index) = states(i);
end
% round 是四舍五入
msecStore(index) = round(time*1000);
index = index + 1;

statesStoreOut = statesStore;
msecStoreOut =  msecStore;

end

测量值预处理整体框架如下：

做了手动开关等处理，这里不多描述工程相关，只梳理算法流程。做了处理的有GPS数据和IMU数据，GPS的处理如下：主要得到水平位置和NED速度

%#codegen
function [VelNED,PosNE]   = ConvertGpsData(GPS_DataArrived,RefLatLongDeg,LatLongDeg,GndSpd,CourseDeg,VelD)

% Do a conversion from lat,long to local North.Est position relative to a
% reference point using WGS-84 eaerth radius and using a simple spherical
% earth model
% Convert GPS course and speed to North and East velocity components

deg2rad = single(pi/180);
earthRadius = single(6378145); %地球半径/米

PosNE = single(zeros(2,1));
VelNED = single(zeros(3,1));
% 当前经纬度减home点的角度，地球半径×弧度角得到相对于原点的距离
%这里分解成home点NE
LatLongDelta = (LatLongDeg-RefLatLongDeg) * deg2rad;
PosNE(1) = earthRadius * LatLongDelta(1);
PosNE(2) = earthRadius * cos(RefLatLongDeg(1)*deg2rad) * LatLongDelta(2);
%水平速X航向角，%航向角是怎么得到的？
VelNED(1) = GndSpd*cos(CourseDeg*deg2rad);
VelNED(2) = GndSpd*sin(CourseDeg*deg2rad);
VelNED(3) = VelD;
return

IMU处理如下：主要通过梯形积分得到增量角度和速度

%#codegen
function [delAngIMU,delVelIMU]  = CalcDeltaAngVel(angRateIMU, accelIMU, dtIMU)

%persistent意味着不需要对其初始化，被定义的变量在函数运行结束后不会被清除
persistent prevAngRateIMU;
if isempty(prevAngRateIMU)
    prevAngRateIMU = single([0;0;0]);
end
persistent prevAccelIMU;
if isempty(prevAccelIMU)
    prevAccelIMU = single([0;0;0]);
end

% Apply trapezoidal integration
%梯形积分得到Δ角和速度
delAngIMU = 0.5*dtIMU*(angRateIMU + prevAngRateIMU);
delVelIMU = 0.5*dtIMU*(accelIMU   + prevAccelIMU  );

prevAngRateIMU = angRateIMU;
prevAccelIMU   = accelIMU;

end

到此，经过处理的数据梳理完毕，其他的都未预处理，直接输出到后续步骤当中。