^2);

fn = DCMbn * (imu.fb(1,:)' - ab_dyn - imu.ab_sta');
wn = DCMbn * (imu.wb(1,:)' - gb_dyn - imu.gb_sta');

% Vector to update matrix F
upd = [vor_dme.vel(1,:) vor_dme.lat(1) vor_dme.h(1) fn' wn'];

% Update matrices F and G
[kf.F, kf.G] = F_update(upd, DCMbn, imu);

[RM,RN] = radius(vor_dme.lat(1));
Tpr = diag([(RM + vor_dme.h(1)), (RN + vor_dme.h(1)) * cos(vor_dme.lat(1)), -1]);  % radians-to-meters

% Update matrix H
kf.H = [ O I O O O ; O O Tpr O O ; ];
kf.R = diag([vor_dme.stdv vor_dme.stdm]).^2;
kf