【PX4 EKF simulink仿真程序解析】（二）先验估计及先验协方差更新
现在来看预测阶段。
先验估计状态更新
%#codegen
function [nextStates,correctedDelAng,correctedDelVel,accNavMag] = UpdateStrapdownEquationsNED( ...
states, ...
earthRateNED, ...
dAngIMU, ...
dVelIMU, ...
dtIMU)
% define persistent variables for previous delta angle and velocity which
% are required for sculling and coning error corrections
persistent prevDelAng;
if isempty(prevDelAng)
prevDelAng = single([0;0;0]);
end
persistent Tbn;
if isempty(Tbn)
Tbn = single(eye(3));
end
gravityNED = [0.0;0.0;9.80665];
% Remove sensor bias errors
%减去偏置得到修正后的增量角度和速度
correctedDelAng = dAngIMU - states(11:13);
correctedDelVel = dVelIMU - states(14:16);
% Save current measurements
prevDelAng = correctedDelAng;
% Apply corrections for earths rotation rate and coning errors
%补偿论文两个地方，一个是地球自转影响，一个是圆锥误差
%旋转过程的旋转轴并非固定产生圆锥误差，大小为1/12 * （correctedAng x Ang）
%大小为什么是这个还需要深究
correctedDelAng = correctedDelAng - transpose(Tbn)*earthRateNED*dtIMU + 8.333333333333333e-2*cross(prevDelAng , dAngIMU);
% Initialise the updated state vector
nextStates = states;
% Convert the rotation vector to its equivalent quaternion
%目的，将旋转量转为四元数
%首先，取模得到旋转角，单位化得到旋转轴
rotationMag = sqrt(correctedDelAng(1)^2 + correctedDelAng(2)^2 + correctedDelAng(3)^2);
%如果转角过小，则不更新四元数
if rotationMag<1e-12
deltaQuat = single([1;0;0;0]);
else
% 轴角转四元数公式
deltaQuat = [cos(0.5*rotationMag); correctedDelAng/rotationMag*sin(0.5*rotationMag)];
end
% Update the quaternions by rotating from the previous attitude through
% the delta angle rotation quaternion
%状态四元数左乘增量四元数，
%得到的四元数的旋转意义表示为坐标系从home点一点一点旋转到当前时刻，Qnb
%Qnb转旋转矩阵得到Tbn，格外注意nb顺序相反。
%原因为，Qnb代表着把n坐标系旋转到b坐标系，因此在表示向量旋转时，就表示为b到n
qUpdated = [states(1)*deltaQuat(1)-transpose(states(2:4))*deltaQuat(2:4); states(1)*deltaQuat(2:4) + deltaQuat(1)*states(2:4) + cross(states(2:4),deltaQuat(2:4))];
% Normalise the quaternions and update the quaternion states
quatMag = sqrt(qUpdated(1)^2 + qUpdated(2)^2 + qUpdated(3)^2 + qUpdated(4)^2);
%如果四元数的模很小是什么含义
%小于1e-16几乎不可能吧，double型有这么多位小数？？
if (quatMag < 1e-16)
nextStates(1:4) = qUpdated;
else
nextStates(1:4) = qUpdated / quatMag;
end
% Calculate the body to nav cosine matrix
%四元数转旋转矩阵
Tbn = [nextStates(1)^2 + nextStates(2)^2 - nextStates(3)^2 - nextStates(4)^2, 2*(nextStates(2)*nextStates(3) - nextStates(1)*nextStates(4)), 2*(nextStates(2)*nextStates(4) + nextStates(1)*nextStates(3));...
2*(nextStates(2)*nextStates(3) + nextStates(1)*nextStates(4)), nextStates(1)^2 - nextStates(2)^2 + nextStates(3)^2 - nextStates(4)^2, 2*(nextStates(3)*nextStates(4) - nextStates(1)*nextStates(2));...
2*(nextStates(2)*nextStates(4)-nextStates(1)*nextStates(3)), 2*(nextStates(3)*nextStates(4) + nextStates(1)*nextStates(2)), nextStates(1)^2 - nextStates(2)^2 - nextStates(3)^2 + nextStates(4)^2];
% transform body delta velocities to delta velocities in the nav frame
%减去偏置补偿重力加速度
delVelNav = Tbn * correctedDelVel + gravityNED*dtIMU;
% calculate the magnitude of the nav acceleration (required for GPS
% variance estimation)
%加速度模值，用于GPS方差估计。
%这里的Δ值都来自于IMU，是要怎么去估算GPS的方差？
accNavMag = sqrt(delVelNav(1)^2 + delVelNav(2)^2 + delVelNav(3)^2) / dtIMU;
% If calculating position save previous velocity
lastVelocity = nextStates(5:7);
% Sum delta velocities to get velocity after removing gravitational skew and correcting for transport rate
%速度更新
nextStates(5:7) = states(5:7) + delVelNav;
% If calculating postions, do a trapezoidal integration for position
%速度梯形积分预测位置
nextStates(8:10) = states(8:10) + 0.5*(nextStates(5:7) + lastVelocity)*dtIMU;
% Copy the remaining states across
%其他几个状态量不变
nextStates(11:24) = states(11:24);
end
先验协方差更新
%#codegen
function nextP = CovariancePrediction(deltaAngle, ...
deltaVelocity, ...
states,...
P, ... % Previous state covariance matrix
dtIMU, ... % IMU and prediction time step
onGround, ... % Boolean to indicate when on ground
useAirspeed, ... % Boolean to indicate that airspeed sensing is being used
useCompass) % Boolean to indicate that a magnetometer is being used
%这个时计算协方差的耗时，这里设为0了，实际应该有一个非零初始化，并用于下方做判断
dtCovPred = 0.0; % time between covariance prediction updates
persistent storedDelAng;
if isempty(storedDelAng)
storedDelAng = single([0;0;0]);
end
persistent storedDelVel;
if isempty(storedDelVel)
storedDelVel = single([0;0;0]);
end
persistent dt;
if isempty(dt)
dt = single(0.0);
end
persistent skipCount;
if isempty(skipCount)
skipCount = 0;
end
%Δ角度&速度累加，这里的角度相加是什么含义？答：应该是状态值来前去这个值，得到P更新前的state
%每次的旋转角都是基于当前时刻机体坐标系得到的，每次Δ角的累加类似于 Q1 + Q2 + ...+Qn
%这两个值并未被调用，是何用处？？？？答：应该是和dtCovPred = 0.0; 有关，非零时应该会被调用
storedDelAng = storedDelAng + deltaAngle;
storedDelVel = storedDelVel + deltaVelocity;
dt = dt + dtIMU;
%IMU的采样时间必须大于下方差的计算时间，才进行更新P
%这里相当于把卡尔曼计算频率降低到dtCovPred，而不是dtIMU
%目前dtCovPred=0，是符合公式的
%但如果dtCovPred=n*dtIMU，这里输入的状态量是state（n-1），用这个状态量的值代入那就不符合公式了
%因此考虑上述两个为用到的变量是为了做补偿，
%因此当上述情况出现，如果是我，我将考虑做如下处理
% persistent firststate;
% if isempty(firststate)
% firststate = state;
% end
% P更新时调用firststate里的数据，其他和下述P更新相同
% 更新完成之后empty（firststate）
if dt >= dtCovPred
% Set the filter process noise - this consists of a very small amount to
% cover processing errors. IMU errors are already built into the
% equations
% It includes the process noise required for evolution of the IMU bias
% errors, wind velocity and magnetic field states
%过程噪声初始化
windVelSigma = single(dt*0.1); % 0.1 m/s/s
dAngBiasSigma = single(dt*0.05/3600*pi/180);
dVelBiasSigma = single(dt*0.01/60);
magEarthSigma = single(dt*10/60); % 10 mgauss per minute
magBodySigma = single(dt*100/60); % 100 mgauss per minute - allow to adapt more rapidly due to changes in airframe
processNoise = [1e-9*ones(1,10), dAngBiasSigma*[1 1 1], dVelBiasSigma*[1 1 1], windVelSigma*[1 1], magEarthSigma*[1 1 1], magBodySigma*[1 1 1]].^2;
% Specify the estimated errors on the delta angles and delta velocities
daxCov = (dt*25.0/60*pi/180)^2;
dayCov = (dt*25.0/60*pi/180)^2;
dazCov = (dt*25.0/60*pi/180)^2;
dvxCov = (dt*0.5)^2;
dvyCov = (dt*0.5)^2;
dvzCov = (dt*0.5)^2;
dvx = deltaVelocity(1);
dvy = deltaVelocity(2);
dvz = deltaVelocity(3);
dax = deltaAngle(1);
day = deltaAngle(2);
daz = deltaAngle(3);
q0 = states(1);
q1 = states(2);
q2 = states(3);
q3 = states(4);
vn = states(5);
ve = states(6);
vd = states(7);
pn = states(8);
pe = states(9);
pd = states(10);
dax_b = states(11);
day_b = states(12);
daz_b = states(13);
dvx_b = states(14);
dvy_b = states(15);
dvz_b = states(16);
vwn = states(17);
vwe = states(18);
magN = states(19);
magE = states(20);
magD = states(21);
magX = states(22);
magY = states(23);
magZ = states(24);
% Predicted covariance
%%%%%%%%%%%%代码太多，不复制了，就是jacobian生成的%%%%%%%
% Add the process noise
%加上过程噪声，这里没用上噪声驱动矩阵，直接加上了
for i = 1:24
nextP(i,i) = nextP(i,i) + processNoise(i);
end
%如果检测到处于地面上，则给速度、风速、磁力计方差赋0值
%如果相应传感器不被使用，同样赋0值
% If on ground, inhibit accelerometer bias updates by setting the coresponding
% covariance terms to zero. To be efficient, the corresponding terms should
% also not be calculated above
if onGround
% Inhibit Acc bias state updates and covariance growth
nextP(14:16,:) = single(0);
nextP(:,14:16) = single(0);
end
% If on ground or no magnetometer fitted, inhibit magnetometer bias updates by
% setting the coresponding covariance terms to zero. To be efficient, the
% corresponding terms should also not be calculated above
if onGround || ~useCompass
% Inhibit magnetic flux state updates and covariance growth
nextP(19:24,:) = single(0);
nextP(:,19:24) = single(0);
end
% If on ground or not using airspeed sensing, inhibit wind velocity
% covariance growth by setting the coresponding covariance terms to zero.
% To be % efficient, the corresponding terms should also not be calculated above
if onGround || ~useAirspeed
% Inhibit Wind velocity state updates and covariance growth
nextP(17:18,:) = 0.0;
nextP(:,17:18) = 0.0;
end
% If the total position variance exceds 1E6 (1000m), then stop covariance
% growth by setting the predicted to the previous values
% This prevent an ill conditioned matrix from occurring for long periods
% without GPS
%防止丢失GPS数据时，位置发散
if (P(8,8) + P(9,9)) > 1E6
nextP(8:9,:) = P(8:9,:);
nextP(:,8:9) = P(:,8:9);
end
% Force symmetry on the covariance matrix to prevent ill-conditioning
% of the matrix which would cause the filter to blow-up
%对角化协方差矩阵
for i = 2:24
for j = 1:(i-1)
temp = 0.5*(nextP(i,j) + nextP(j,i));
nextP(i,j) = temp;
nextP(j,i) = temp;
end
end
% reset the accumulated values
% 更新完P后这几个值赋为0，和上述猜测一样，如果dtCovPred =n*dtIMU，角度和速度是需要补偿的;
storedDelAng = single([0;0;0]);
storedDelVel = single([0;0;0]);
dt = single(0.0);
else
nextP = P;
end
end
速度、位置、磁力计、空速，从stateStore中取一个最优时刻状态，用于融合
这五项用的是同一个函数：
%#codegen
function [fuseData,statesOut] = RecallStates1(statesStore,msecStore,msecDelay,states,time,newDataArrived)
persistent statesForFusion;
if isempty(statesForFusion)
statesForFusion = single(zeros(24,1));
end
%这个Delay是什么？答：是一个初始化值
bestTimeDelta = int32(msecDelay);
bestStoreIndex = 1;
%如果GPS数据送达
if newDataArrived
for storeIndex=1:50
%当前时间-速度延时-储存状态的时间戳
timeDelta = (int32(time*1000) - int32(msecDelay)) - int32(msecStore(storeIndex));
%Delta = NowTime - stateStoreTime
%if Delta < msecDelay: Deltatime = |Delta - msecDelay|
%还剩多久能到msecDelay
if (timeDelta < 0)
timeDelta = -timeDelta;
end
% if msecDelay < Delta < 2*msecDelay
%最佳索引和最佳时间 = Delta - msecDelay
if (timeDelta < bestTimeDelta)
bestStoreIndex = storeIndex;
bestTimeDelta = timeDelta;
end
end
%比只输出当前状态的精度好，大概率输出的最近一次的后验估计值
if (bestTimeDelta < msecDelay) % only output stored state if time accuracy better than just outputtting current states
for i=1:24
statesForFusion(i) = statesStore(i,bestStoreIndex);
end
%如果2*msecDelay时间范围内没有新的后验状态输入
%那么就取最近的一次先验估计状态量
else % otherwise output current state
for i=1:24
statesForFusion(i) = states(i);
end
end
end
statesOut = statesForFusion;
fuseData = newDataArrived;
end
如果没有这个设计，直接取最近的一次状态值，感觉影响不大，待后续日志数据验证
853
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
