propagate_fej.m

function [xe, Pe,xR_k_k1, PHI_fej,G_fej,PHI_mult] = propagate_fej(xe,Pe,dt,v_m,omega_m,sigma_v,sigma_w,xR_k_k1,PHI_mult)


J = [0 -1; 1 0];
Q = [sigma_v^2  0;    0    sigma_w^2];

xR_k1_k1 = xe(1:3,1);



% % propagate state, k+1|k

xe(1:3,1) = [ xe(1) + v_m*dt*cos(xe(3));
    xe(2) + v_m*dt*sin(xe(3));
    pi_to_pi(xe(3) + omega_m*dt) ];


% PHI evluated at: k+1|k - k|k-1
PHI = [ eye(2)   J*(xe(1:2,1)-xR_k_k1(1:2,1));
    zeros(1,2)  1 ];


% G regularly evaluated at k-1|k-1
G = [dt*cos(xR_k1_k1(3)) 0;
    dt*sin(xR_k1_k1(3)) 0;
    0   dt ];


Qprime = G*Q*G';
Qprime = compute_Q(xR_k1_k1,dt,v_m,omega_m,sigma_v,sigma_w);


% propagate covariance
Pe(1:3,1:3) = PHI*Pe(1:3,1:3)*PHI' + Qprime;
if size(Pe,1)>3
    Pe(1:3,4:end) = PHI*Pe(1:3,4:end);
    Pe(4:end,1:3) = Pe(1:3,4:end)';
end
Pe = 0.5*(Pe+Pe');

% % % 
xR_k_k1 = xe(1:3,1);


% % 
PHI_fej = blkdiag(PHI,eye(size(xe,1)-3));
G_fej = [G; zeros(size(xe,1)-3,2)];


% % used for hybrid FEJ-OCUKF
% % PHI_mult=PHI(k+m-1)*...*PHI(k+1)*PHI(k), used in computing nullspace of H
PHI_fejukf = blkdiag(PHI,eye(size(xe,1)-3));%%%%
if size(PHI_fejukf,1)>size(PHI_mult,1);
    PHI_mult = blkdiag( PHI_mult,eye(size(PHI_fejukf,1)-size(PHI_mult,1)) );
end
PHI_mult = PHI_fejukf*PHI_mult;