You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
60 lines
1.4 KiB
60 lines
1.4 KiB
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 |