admin管理员组文章数量:1619183
融合部分
- GPS融合
- 考虑GPS延迟情况,选取离IMU当前采样时刻距离最近的GPS的索引latest_gps_index
- 判断GPS是否blocked
- 初次使用GPS信息,判断速度方差是否小于gpsSpdErrLim(设为1),位置方差是否小于gpsPosErrLim(设为5)。使用GPS的水平位置、速度进行初始化初始化后,将gps_use_started 设为true,以后不再进行初始化。
states(5:7) = gps_data.vel_ned(latest_gps_index,:);
states(8:9) = gps_data.pos_ned(latest_gps_index,1:2);
gps_use_started = true;
last_drift_constrain_time = local_time;
- 判断
gps_use_started && ~gps_use_blocked && (latest_gps_index > last_gps_index
,满足则融合GPS数据。- GPS速度、位置融合,不仅对速度位置修正,其他相关均修正
- GPS相关索引更新
last_gps_index = latest_gps_index;
gps_fuse_index = gps_fuse_index + 1;
last_drift_constrain_time = local_time;
- 速度融合
- 速度融合函数
FuseVelocity()
,输入状态量为一步预测获得一步预测状态量和一步预测协方差,GPS速度量测,GPS速度的Gate(设为5),GPS速度量测的方差,输出为状态量,协方差,速度的新息,速度新息的协方差。- 初始化
innovation = zeros(1,3);
varInnov = zeros(1,3);
H = zeros(3,24);
- 新息innovation、雅克比矩阵、新息协方差varInnov的计算
for obsIndex = 1:3
stateIndex = 4 + obsIndex;
速度对应的状态索引(5:7)
innovation(obsIndex) = states(stateIndex) - measVel(obsIndex);
速度量测新息
H(obsIndex,stateIndex) = 1;
观测雅克比矩阵
varInnov(obsIndex) = (H(obsIndex,:)*P*transpose(H(obsIndex,:)) + R_OBS);
速度新息的协方差 - NED量测速度与门限值的比较判断
(innovation(obsIndex)^2 / (gateSize^2 * varInnov(obsIndex))) > 1.0
- 计算kalman增益、状态和协方差,循环1:3
K = (P*transpose(H(obsIndex,:)))/varInnov(obsIndex);
增益
xk = K * innovation(obsIndex);
states = states - xk;
状态
P = P - K*H(obsIndex,:)*P;
协方差
P = 0.5*(P + transpose(P));
保证协方差的对称性
if P(i,i) < 0 P(i,i) = 0; end
对角线负值置0
- 初始化
- 速度融合数据记录
output.innovations.vel_time_lapsed(gps_fuse_index) = local_time; output.innovations.vel_innov(gps_fuse_index,:) = velInnov'; output.innovations.vel_innov_var(gps_fuse_index,:) = velInnovVar';
- 速度融合函数
- 位置融合
FusePosition()
,与速度融合相一致
- GPS相关索引更新
- 如果不满足条件,首先判断GPS是否5s数据都没有参与融合,如果是,则按照5Hz进行零位置修正。
- GPS速度、位置融合,不仅对速度位置修正,其他相关均修正
- *气压计融合
- 考虑延迟,计算最近的气压计数据ID
latest_baro_index = find((baro_data.time_us-1e6 * param.fusion.baroTimeDelay) < imu_data.time_us(imuIndex),1, 'last')
- 当前状态更新
last_baro_index = latest_baro_index;
baro_fuse_index = baro_fuse_index + 1;
- 融合高度计信息
FuseBaroHeight()
,与GPS速度融合相似,观测方差设为3,门限值设为5。 - 融合后的数据输出
output.innovations.hgt_time_lapsed(baro_fuse_index) = local_time;
output.innovations.hgtInnov(baro_fuse_index) = hgtInnov;
output.innovations.hgtInnovVar(baro_fuse_index) = hgtInnovVar;
- 考虑延迟,计算最近的气压计数据ID
- 磁航向融合
- 考虑数据延迟,计算最近的磁力计数据ID
latest_mag_index = find((mag_data.time_us - 1e6 * param.fusion.magTimeDelay) < imu_data.time_us(imuIndex), 1, 'last' )
- 当前状态更新
last_mag_index = latest_mag_index;
mag_fuse_index = mag_fuse_index + 1;
- 三轴磁模量计算并输出
output.innovations.magLength(mag_fuse_index) = sqrt(dot(mag_data.field_ga(latest_mag_index,:),mag_data.field_ga(latest_mag_index,:)));
- 按照磁航向融合param.fusion.magFuseMethod == 2
FuseMagHeading()
输入为状态量,协方差,三轴磁力计数据,当地磁偏角转换弧度,Hdg门限值5,Hdg误差0.1745- 获取states四元数
q0 = states(1); q1 = states(2); q2 = states(3); q3 = states(4);
和磁力计数据magX = magData(1); magY = magData(2); magZ = magData(3);
- 建立磁航向的量测矩阵
H = calcH_HDG(magX,magY,magZ,q0,q1,q2,q3);
- 计算总方差和卡尔曼增益
varInnov = (H*P*transpose(H) + R_MAG); Kfusion = (P*transpose(H))/varInnov;
- 新息计算
应用当前四元数计算捷联矩阵Tbn = Quat2Tbn(states(1:4));
将磁力计输出转到地理系magMeasNED = Tbn*[magX;magY;magZ];
应用地理系下的磁力计输出计算磁偏角
predDec = atan2(magMeasNED(2),magMeasNED(1));
计算信息:当地磁偏角与磁力计获得的相减
innovation = predDec - measDec;
- 新息判断和限制
新息限制在-pi~+pi之间:
if (innovation > pi) innovation = innovation - 2*pi; elseif (innovation < -pi) innovation = innovation + 2*pi; end
新息和innovGate判断:
if (innovation^2 / (innovGate^2 * varInnov)) > 1.0 innovation = NaN; varInnov = NaN; return; end
- 状态更新
states = states - Kfusion * innovation;
- 四元数归一化
quatMag = sqrt(states(1)^2 + states(2)^2 + states(3)^2 + states(4)^2);
if (quatMag > 1e-12) states(1:4) = states(1:4) / quatMag; end
- 计算协方差P
P = P - Kfusion*H*P;
P = 0.5*(P + transpose(P));
保证对称性
for i=1:24 if P(i,i) < 0 P(i,i) = 0; end
对角线为负则置0
- 获取states四元数
- 输出数据保存
output.innovations.mag_time_lapsed(mag_fuse_index) = local_time;
output.innovations.hdgInnov(mag_fuse_index) = hdgInnov;
output.innovations.hdgInnovVar(mag_fuse_index) = hdgInnovVar;
- 考虑数据延迟,计算最近的磁力计数据ID
至此全部融合结束,输出时间更新,输出平均时间
output.dt = dtCovInt / covIndex;
dtCovInt 为累积的时间,covIndex为累积的帧数
版权声明:本文标题:ekf2 MATLAB离线版本详解(三) 内容由热心网友自发贡献,该文观点仅代表作者本人, 转载请联系作者并注明出处:https://www.elefans.com/dongtai/1728792271a1173826.html, 本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌抄袭侵权/违法违规的内容,一经查实,本站将立刻删除。
发表评论