function P = PredictCovariance(deltaAngle, ... deltaVelocity, ... quat, ... states,... P, ... % Previous state covariance matrix dt) ... % IMU and prediction time step % Set the filter state process noise (IMU errors are already built into the % equations). It includes the process noise required for evolution of the % IMU bias errors dAngBiasSigma = dt*1E-6; processNoise = [0*ones(1,7), dAngBiasSigma*[1 1 1]]; % Specify the estimated errors on the delta angles and delta velocities daxNoise = (dt*25.0/60*pi/180)^2; dayNoise = (dt*25.0/60*pi/180)^2; dazNoise = (dt*25.0/60*pi/180)^2; dvxNoise = (dt*0.5)^2; dvyNoise = (dt*0.5)^2; dvzNoise = (dt*0.5)^2; dvx = deltaVelocity(1); dvy = deltaVelocity(2); dvz = deltaVelocity(3); dax = deltaAngle(1); day = deltaAngle(2); daz = deltaAngle(3); q0 = quat(1); q1 = quat(2); q2 = quat(3); q3 = quat(4); dax_b = states(8); day_b = states(9); daz_b = states(10); % Predicted covariance F = calcF(dax,dax_b,day,day_b,daz,daz_b,dvx,dvy,dvz,q0,q1,q2,q3); Q = calcQ(daxNoise,dayNoise,dazNoise,dvxNoise,dvyNoise,dvzNoise,q0,q1,q2,q3); P = F*P*transpose(F) + Q; % Add the general process noise for i = 1:10 P(i,i) = P(i,i) + processNoise(i)^2; end % Force symmetry on the covariance matrix to prevent ill-conditioning % of the matrix which would cause the filter to blow-up P = 0.5*(P + transpose(P)); % ensure diagonals are positive for i=1:9 if P(i,i) < 0 P(i,i) = 0; end end end