【PX4 EKF simulink仿真程序解析】（二）先验估计及先验协方差更新

【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

如果没有这个设计，直接取最近的一次状态值，感觉影响不大，待后续日志数据验证