

<span class="comment">% Update matrices F and G</span>
[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]);  <span class="comment">% radians-to-meters</span>

<span class="comment">% Update matrix H</span>
kf.H = [ O I O O O ; O O Tpr O O ; ];
kf.R = diag([vor_dme.stdv vor_dme.stdm]).^2;
kf.z = [ vor_dme.stdv, vor_dme.stdm ]';

<span class="comment">% Propagate prior estimates to get xp(1) and Pp(1)</