ekf2 MATLAB离线版本详解（一）

主循环函数RunFilter(），输入param,imu_data,mag_data,baro_data,gps_data

初始化设置

• InitStates(),输入param,imu_data,mag_data,baro_data,gps_data
  此函数为导航初始对准时所必需进行的步骤，输出为states的24个状态变量，imu_start_index为*imu_*开始的索引。
  • param.control.waitForGps此标准位为1时表示需要等到GPS有效时才进行初始对准。一般情况下不需要等待gps，所以imu_start_index=1。
  • 姿态角的初始对准，states(1:4)对应状态量的姿四元数，初始值设为quat=[1;0;0;0].
    • 计算前100个imu速度增量的平均值以及采样时间间隔的平均值，将两个值相除，获得平均的加速度输出initAccel(1:3)。
    • AlignTilt()，输入为quat，initAccel。
      计算initAccel的模，如果大于5(0.5g)，小于14(1.4g)，则进行对准，否则退出。
      计算IMU水平两轴的模值与z轴的比值，计算俯仰角。
      如果俯仰角大于1e-3，使用叉乘计算法向量，并进行归一化得到tiltUnitVec。
      基于旋转向量和旋转角的公式计算四元数。
      quat_align_err=EulToQuat();
      输入参数为param.control.rollAlignErr，param.control.pitchAlignErr，param.control.yawAlignErr*
      考虑四元数初始化误差，但是初始化的这些参数都是0，所以这部分未修正。
  • 航向的初始对准
  • 确定磁力计初始对准的index，使用磁力计的时间戳大于imu初始对准的时间戳的第一帧，取周围的10个进行均值计算，magBody(1,1),magBody(2,1),magBody(3,1)。开始会出现数据不稳定的情况。
    AlignHeading()，输入参数为quat,magBody,param.fusion.magDeclDegdeg2rad
    quat为当前四元数，magBody为上述计算的磁力计的均值，magDeclDeg为根据当地经纬度计算的磁偏角（地理北极与地磁北极的夹角）。
    * 具体实现：
    * 根据当前四元数计算当前欧拉角，随后将航向角置0。
    euler = QuatToEul(quat);euler(3) = 0.0;
    * 将新的姿态角转换为四元数，再转成捷联矩阵。
    quat_zero_yaw = EulToQuat(euler);
    Tbn_zero_yaw = Quat2Tbn(quat_zero_yaw);
    * 将磁力计均值转换到地理坐标系下magMeasNED。
    euler(3) = declination - atan2(magMeasNED(2),magMeasNED(1));
    declination 为当地磁偏角，atan2的范围是-pi~+pi，取负号，转为世界坐标系下以当前为基准的航向，北偏东为正，反之为负。
    * 再将欧拉角转换为四元数
  • 将当前四元数转化为旋转矩阵，NED系下的三轴分量作为states(17:19)。Tbn = Quat2Tbn(quat);
    states(17:19) = Tbn*magBody;
  • 三轴速度*states(5:7)和水平位置states(8:9)*的初始化
    • 在等待GPS的情况下，使用GPS的位置和速度进行初始化。
    • 在不等待GPS情况下，初始化为0。
  • 高度的初始化states(10)。
    • 找到气压计跟开始imu索引对应的index，baro_start_index
    • 使用气压计前20个数的均值对states(10)进行初始化。
• 相关变量的初始化
  • dt_imu_avg = 0.5 * (median(imu_data.gyro_dt) + median(imu_data.accel_dt));
    计算陀螺和加速度计采样时间的均值，取平均。
  • indexStop = length(imu_data.time_us) - imu_start_index; indexStart = 1;
  • 创建输出变量的存储空间
    output = struct('time_lapsed',[]',... 'euler_angles',[],... 'velocity_NED',[],... 'position_NED',[],... 'gyro_bias',[],... 'accel_bias',[],... 'mag_NED',[],... 'mag_XYZ',[],... 'wind_NE',[],... 'dt',0,... 'state_variance',[],... 'innovations',[],... 'magFuseMethod',[]);
• 初始化协方差矩阵covariance = InitCovariance(param,dt_imu_avg,1,gps_data);
  1表示gps_alignment=1，表示使用GPS的位置和速度误差进行初始化
  • 姿态四元数的协方差初始化
    Sigma_quat = param.alignment.quatErr * [1;1;1;1];quatErr=0.1.
  • 速度协方差的初始化，使用GPS初始索引对应的速度误差进行初始化Sigma_velocity = gps_data.spd_error(gps_data.start_index) * [1;1;1];
    spd_error为GPS速度的方差。
  • 位置协方差的初始化，使用GPS初始索引对应水平的位置误差进行初始化 Sigma_position = gps_data.pos_error(gps_data.start_index) * [1;1;0] + [0;0;param.alignment.hgtErr];
    hgtErr是设置的高度误差方差，为0.5
  • 陀螺漂移协方差初始化
    Sigma_dAngBias = param.alignment.delAngBiasErr*dt*[1;1;1];
    delAngBiasErr设置为0.05*pi/180，*dt变为角度偏置
  • 加速度计零偏协方差初始化
    Sigma_dVelBias = param.alignment.delVelBiasErr*dt*[1;1;1];
    delVelBiasErr设置为0.07，*dt变为速度偏置
  • 磁力计NED，XYZ的初始化
    magErrNED为0.5，magErrXYZ为0.01
  • 风速误差协方差初始化
    windErrNE为5
  • 将以上各个状态量的协方差组成对角阵

至此，初始化部分结束