  <span class="comment">% Error vector state</span>
kf.Pi = diag([imu.ini_align_err, vor_dme.stdv, vor_dme.std, imu.gb_dyn, imu.ab_dyn].^2);

kf.Q  = diag([imu.arw, imu.vrw, imu.gb_psd, imu.ab_psd].^2);
<span class="comment">%kf.Q  = diag([imu.arw, imu.vrw, [0 0 0], [0 0 0]].^2);</span>

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

<span class="comment">% Vector to update matrix F</span>
upd = [vor_dme.vel(1,:) vor_dme.lat(1) vor_dme.h(1) fn' wn'];