主循环Main Loop
-
初始化设置融合标志位，设为非
gps_use_started = boolean(false); gps_use_blocked = boolean(false); viso_use_blocked = boolean(false); flow_use_blocked = boolean(false); -
涉及的传感器索引的初始化
imuIndex = imu_start_index; last_gps_index = 0; gps_fuse_index = 0; last_baro_index = 0; baro_fuse_index = 0; last_mag_index = 0; mag_fuse_index = 0; last_flow_index = 0; flow_fuse_index = 0; last_viso_index = 0; viso_fuse_index = 0; last_range_index = 0; -
一步预测协方差变量的初始化
delAngCov = [0;0;0];一步预测协方差时的角度方差
delVelCov = [0;0;0];一步预测协方差时的速度方差
dtCov = 0;一步预测协方差时的时间间隔
dtCovInt = 0;一步预测协方差时累计的时间间隔
covIndex = 0;一步预测协方差时帧的统计 -
选择磁力计融合的方式
output.magFuseMethod = param.fusion.magFuseMethod;
range = 0.1; -
控制位姿解算错误的时间的初始化
last_drift_constrain_time = - param.control.velDriftTimeLim;velDriftTimeLim为5
last_synthetic_velocity_fusion_time = 0;
last_valid_range_time = - param.fusion.rngTimeout;rngTimeout为2 -
主循环
index = indexStart:indexStop- 读取IMU的数据
* 获取IMU的数据并换算成秒
local_time=imu_data.time_us(imuIndex)*1e-6;
* 获取IMU的角速度和加速度
delta_angle(:,1) = imu_data.del_ang(imuIndex,:);
delta_velocity(:,1) = imu_data.del_vel(imuIndex,:);
* 获取IMU的时间间隔，对陀螺仪和加速度计的时间间隔求平均
dt_imu = 0.5 * (imu_data.accel_dt(imuIndex) + imu_data.gyro_dt(imuIndex));
*imuIndex = imuIndex+1; - 一步预测状态
输入值为上一时刻的状态，当前时刻的陀螺仪和加速计输出，时间间隔，当地重力加速度，GPS经纬度，输出为当前时刻状态，角度和速度的修正值。
[states, delAngCorrected, delVelCorrected] = PredictStates(states,delta_angle,delta_velocity,imu_data.accel_dt(imuIndex),gravity,gps_data.refLLH(1,1)*deg2rad);- 存储前一时刻的速度变化、角度变化、旋转矩阵prevDelAng、prevDelVel、Tbn_prev
- 消除加速度零偏和陀螺漂移，上一时刻的漂移
delAng = delAng - states(11:13);
delVel = delVel - states(14:16); - 计算当前纬度下的地球自传角速度引起的角度在地理系下的分量
delAngEarth_NED(1,1) = 0.000072921 * cos(latitude) * dt;
delAngEarth_NED(2,1) = 0.0;
delAngEarth_NED(3,1) = -0.000072921 * sin(latitude) * dt; - 将地球自转的角速度引起的角度转换到在体系下对陀螺输出补偿，得到载体相对于地理系的角度变化。
correctedDelAng = delAng - transpose(Tbn_prev)*delAngEarth_NED; - 存储当前的角度和速度 变化量
prevDelAng = delAng;prevDelVel = delVel; - 将角度的变化量correctedDelAng应用旋转矢量法计算当前时刻四元数变化量
deltaQuat = RotToQuat(correctedDelAng); - 通过四元数间的乘法，将上一时刻和当前时刻四元数变化量相乘获得当前时刻四元数并归一化
states(1:4) = QuatMult(states(1:4),deltaQuat);当前时刻四元数
states(1:4) = NormQuat(states(1:4));归一化
Tbn = Quat2Tbn(states(1:4));将四元数转化为捷联矩阵
Tbn_prev = Tbn;存储捷联矩阵 - 将加速度计输出通过捷联矩阵转化到载体坐标系上，并补偿重力加速度
delVelNav = Tbn * correctedDelVel + [0;0;gravity]*dt; - 保持上一时刻速度并更新当前速度
prevVel = states(5:7);
states(5:7) = states(5:7) + delVelNav(1:3); - 更新位置信息，使用上一时刻速度和当前速度的平均值计算
states(8:10) = states(8:10)+0.5*dt*(prevVel+states(5:7));
- 对一步预测后的状态进行限幅操作
[states] = ConstrainStates(states,dt_imu_avg); - 时间、角度、速度的修正
dtCov = dtCov + dt_imu;
delAngCov = delAngCov + delAngCorrected;
delVelCov = delVelCov + delVelCorrected; - 判断dtCov 是否大于 0.01，若大于，则
- 方差预测
输入量为角度和速度变化量的协方差，当前时刻状态，上一时刻协方差，修正时间间隔，相关参数。
covariance = PredictCovariance(delAngCov,delVelCov,states,covariance,dtCov,param);- 计算过程噪声
dAngBiasSigma = dt*dt*param.prediction.dAngBiasPnoise;陀螺漂移噪声
dVelBiasSigma = dt*dt*param.prediction.dVelBiasPnoise;加速度计零偏噪声
magSigmaNED = dt*param.prediction.magPnoiseNED;磁力计零偏噪声
magSigmaXYZ = dt*param.prediction.magPnoiseXYZ;XYZ磁零偏噪声
将上述过程噪声组成对角阵processNoiseVariance - 计算一阶泰勒展开的状态转移矩阵和过程噪声阵，即EKF算法。
- 计算一步预测协方差矩阵，并在对角线上加过程噪声 。
P = F*P*transpose(F) + Q;
P(i,i) = P(i,i) + processNoiseVariance(i); - 将P强制对称性
P = 0.5*(P + transpose(P)); - 确保对角线元素为正，不为正的置0
- 计算过程噪声
- 修正量和积分时间均清0
delAngCov = [0;0;0];
delVelCov = [0;0;0];
dtCov = 0; - 时间及帧++
covIndex = covIndex + 1;
dtCovInt = dtCovInt + dtCov; - 输出数据
输出时间为当前IMU时间
output.time_lapsed(covIndex) = local_time;
输出欧拉角估计
output.euler_angles(covIndex,:) = QuatToEul(states(1:4)')';
输出NED速度估计
output.velocity_NED(covIndex,:) = states(5:7)';
输出NED位置估计
output.position_NED(covIndex,:) = states(8:10)';
输出陀螺漂移估计
output.gyro_bias(covIndex,:) = states(11:13)';
输出加速度计零偏估计
output.accel_bias(covIndex,:) = states(14:16)';
输出NED磁力计零偏
output.mag_NED(covIndex,:) = states(17:19);
输出XYZ磁力计零偏
output.mag_XYZ(covIndex,:) = states(20:22);
输出风速估计
output.wind_NE(covIndex,:) = states(23:24);
输出协方差矩阵
output.state_variances(covIndex,i) = covariance(i,i);
输出欧拉角误差，欧拉角协方差矩阵
error_transfer_matrix = quat_to_euler_error_transfer_matrix(states(1),states(2),states(3),states(4));
euler_covariance_matrix = error_transfer_matrix * covariance(1:4,1:4) * transpose(error_transfer_matrix);
output.euler_variances(covIndex,i) = euler_covariance_matrix(i,i);
- 方差预测
- 读取IMU的数据