关于apm ekf2 在ekf 上的改进研究

ekf2在与ekf相比在思想上有了一个值得飞跃，ekf关注的是如何把数据融合进去；ekf2通过互补滤波的思想，重新架构了算法；ekf滤波在ekf中主要是用来需要在imu预测的误差，而不是想在ekf中直接输出姿态、速度、位置。
雷神空大-植保无人机htt://ww. sclskt. com/ 2、对 readIMUData(函数进行分析 void NavEKF2 core: readIMUData( //获取加速度计的积分速度 if (ins. use accel(imu index)) readDeltaVelocity(imu index, imuDataNew delvel, imuDataNew delvelDT) accelPosoffset= ins. get imu pos offset (imu index) clse readDeltaVelocity(ins. get primary accel, imuDataNew delVe imuDataNew. delveldt accel PosOffset ins get imu pos offset (ins. get primary accel o) /获取陀螺仪的积分角度 if (ins. use gyro(imu index) readDel taNgle(imu index, imuDataNew del Ang) ⊥se rcadDeltaAnglc(ins. get primary gyro(, imuDataNcw delAng //记录新的数据时间 imuDataNew time ms imuSamplefime ms //累计积分时间 imuDa taDownSampledNew de langat + imuDataNew de langdt imuDataDown SampledNew de veldt + imuDataNew delveldt //累计积分角度 imuQuat Down SampleNew rotate(imuDataNew delang imuQuatDownSampleNew normalize //求陀螺仪变化的旋转矩阵,目的是把数据的改变投影过米 Matrix3f deltarotMat imuQuatDownSampleNew. rotation matrix(deltaRotMat) //积累积分速度 / apply the delta velocity to the delta velocity accumulator imuDataDownSampledNew. delvel= deltaRotMatkimuDataNew delvel ekf间隔处理imu数据,不用每次都处理,这里处理的频率大概为50Hz,而gps的频 率为 //5Hz,气压计为10hz,所有这些传感器的数据实际是不影响的 雷神空天植保无人札http://www.sclskt.com if ((dtIMUavg*(float)framesSincePredict >=EKF TARGEt dt & startPredictEnabled)(dtIMUavg*(float)frames SincePredict > 2. 0f*EKF TARGET DT))( //通过imu在这段时间中的变化量求出旋转的弧度大小(旋转轴*旋转角度) imuQuat Down SampleNew to axis angle(imuDataDown SampledNew. delAng) /记录采样时间 i muDataDown dNew. time ms- imuSamplcTime ms //记录本次累计采样数据到环形 buffer storedIMU push youngest element(imuDataDownSampledNew) //清空采样累加器 // zero the accumulated imu data and quaternion imuDataDownSampledNew. delAng. zero o) imuDataDownSampledNew del vel zero) imuDataDownSampledNew. delAngDT=0 of imuDataDownSampledNew. delvelDt-0 of imuQuatDownSampleNew[O]=1.0f imuQuat DownSampleNew[3]= imuQuatDownSampleNew_2] imuQuat Down SampleNewl1]=0.0f framesSincePredict=0 //运行状态更新设置 runUpdates-true ∥/读取最久的一次采样,这个跟最新的数据相差了size(13或26)个采样 // extract. the oldest available data from the Fifo buffer imuDataDelayed= storedIMU pop oldest element O // protect against delta time going to zero // TODO- check if calculations can tolerate 0 float minD=0. 1*dtEkfavg imuDataDelayed de lAngD'T=MAX (imuDataDelayed de lAnglI, minDT') imuDataDclayed. dolvelDT-MAX(imuDataDelayed del VelDt, minDT) // correct the extracted imu data for sensor errors de Corrected= imuDataDe layed delang de lvelCorrected= imuDataDe layed devEl correct DeltaAngle(delAngCorrected, imuDataDe layed delangDt) orrectDeltaVelocity(delVelCorrected, imuDataDelayed delVelDT) 4 雷神空天-植保无人机http://www.sclskt.com 3、ca| cOutputstates函数中实现了互补滤波 void NavEKF2 core:: calcOutputStateso) /0号中的代码运行频率是400hz,使用im数据来预测姿态、速度、位置 //获取陀螺仪的积分角度 Vector 3f de lAng NewCorrected =imuDataNew de lAng //获取加速度机的积分速度 Vector 3f delve lNewCorrected imuDataNew devel //修正加速度积分的速度 correctDeltavelocity(delVelNewCorrected, imuDataNew delvelDT) //imu:积分的角度加上ekf的对现在角度的修正值(这里是姿态的互补滤波) Vector 3f delang- delAngNew Corrected delAngCorrection /将计算的三角角度转换成四元素 Quaternion del taQuat deltaQuat. from axis angle(delang); //在前一次输出上进行旋转 outputDataNew quat *-deltaQuat outputDataNew. quat. normalize //求出从body坐标系到nav坐标系的旋转余弦矩阵 Matrix3f Ton temp outputDataNew quat rotation matrix(Tbn temp) //把速度旋转到nav坐标系中 Vector 3f del ve lNav= Tbn temp*delvelNewCorrected delve lnay z + gravity MSS kimudatanew delveld //保存上一次的速度 Vector 3f lastVelocity -outputDataNew velocity //计算当前速度 outputDataNew. velocity + delvelnav //通过开始速度和当前速度计算位置 雷神空天植保无人札http://www.sclskt.com outputDataNew. position +=(outputDataNew velocity lastVelocity)* (imuDataNew. delvelDt*0. 5f) /处理imu是否有位置偏移,3.5版本添加了可以把imu位置偏移到重心上 if(!accelPosOffset is zero o)i /i calculate the average angular rate across the last IMu update // note delangdt is prevented from being zero in readIMUData o Vector 3f ingRate= imuDataNew de lAng *(1 of/imuDataNew delAngDT) 7, Calculate the velocity of the body frame origin relative to the IMU in bod frame // and rotate into earth frame. Note operator has been overloaded to perform a cross product Vector3f velBodyRelIMl-angRate %(accelPosOffset) veloffsetNeD= Tbn temp *k ve bodyRelIMu / calculate the earth frame position of the body frame origin relative to the ⊥MU posOffsctNED- Tbn temp *(accelPosOffsct) , else veloffsetNED. zero posOffsetNED. zero /不使用ekf可补滤波,到这里姿态、速度、位置的预测就完成了 /互补滤波的实现 f (runUpdates /把新的姿态保存到环形υ uffer,这里不是每次郗会保存的,只有运行一次ekf 才会保存一次 storedOutput [storedIMU get youngest index]= outputDataNew //获取最久的一次保存,时间差在0.02s以内 utput DataDelaye toredOutput [stored IMU get olde dex () /求出ekf的姿态与时间最久的姿态差四元素(就是旋转的角度用四元素表示) Quaternion quatErr stateStruct quat outputDataDelayed quat //这个地方是求四元素旋转转换成三角角度表示,做了个近似处理,当 theta很 的时候, theta=sin( theta)= theta/2 quatErr, normalize(; 雷神空天植保无人札http://www.sclskt.com Vector3f del taangerr float scaler if quatEr[0] >=0.of)i der=2.01 scaler=-2 0f deltaAngErr. x- scaler quatErr[l] deltaAngErr y scaler quatErr [2] deltaAngErr.z- scaler x quatErr[3] //这里计算了一个阻尼比,计算方式也很简单,就是:1/2*( dtimlavg/ t imeDelav) float timeDelay- 1c-3f *(float)(imuDataNew time ms imuDataDe layed time ms time Delay- fmaxf(time Delay, dtlmlavg) float errorGain =0.5f , time Delay //计算姿态的修正值,在400hz的循环里用这个值 delangCorrection=deltaAngErr *k errorGain x dtImUavg /计算速度和位置的差 Vector3f velErr =(stateStruct velocity- outputDataDelayed. velocity) Vector 3f posErr =(stateStruct position output DataDelayed position //这里通过设置的时间常数和ekr平均运行的时间相比,求出一个增益 //用这个增益的的二次函数对速度和位置差的积分进行了处理,得到了速度 位置修正值,这里应该有用原始数据仿真过 float tauposvel constrain float(0. 01f*(float)frontend-> tauVelPos Output, 0. If, 0.5f) float velos Gain=dtEkfAvg/constrain float(tauPosVel, dtEkfAvg, 10. 0f) //速度、位置积分 posErrintegral + poser velerrintegral + ve lerr //用了二次幂函数拟合修正值 Vector 3f vel Correction= ve lErr xk velPosGain I velErrintegral *k sa(velos Gain)*0. lf Vector3f pos Correction= poserr *k ve lPosGain poserrintegral x sq(velposGain)*0.1f //用ekf修正更新输出层的位置和速度 output elements outputStates 7 雷神空天-植保无人机htt://www.sclskt.com/ for (unsigned index=0; index< imu buffer length; index++) outputStates= storedOutput [index] a constant velocity correction is applier outputstates, velocity + velCorrection / a constant position correction is applied outputstates. position + pos Correction // push the updated data to the buffer storedOutput [index]- outputStates //更新向外提供的数据 tputDataN [storedIMU get youngest index] 4、上面提到了ekf2的姿态、位置和速度,有必要看下姿态的融合 正常的ekf算法融合是通过测量值,求出革新值,通过革新值求增益,通过增益更新Ek 状态和协方差,无论是ekf和ek2对应四元素的融合思路都是用inu积分预测姿态、速度、 位置,更新协方差;用协方差更新姿态的过程在其他传感器融合的时候顺便做了,这样做可 能是由于imu的高频,连续性不好。如果刚兴趣可以看我的“px4平台之我见”,里面有介 绍融合的实现细节。 用imu数据预测姿态、速度、位置 UpdateStrapdownEquationsNEDO 预测协方差 Covariance Prediction(; void NavEKF2 core: UpdateStrapdownEquationsNEDO // delAng Correcte是ekf记录的最久的角度值,大概跟最新的相差size(13/26)个数据, ∥/大概相差sze*4个采样,这样做可以消除一些瞬时误差,用于跟快速的姿态直接预 测方法形成互补滤波增加系统的稳定性。 这里考虑了地球的自转,把地球自转变化到了body坐标系中 stateStruct. quat. rotate delAng Corrected - prevTnb earth Ratened imu Data Delayed. delAngDT) stateStruct. quat. normalize: // delvelcorrected是加速度计的积分是在body坐标系中完成的, //需要把速度转到nav坐标系,才能求速度和位置。 Vector 3f de lvelnav;// delta velocity vector in earth axes 雷神空天植保无人札http://www.sclskt.com del ve lnav= prevTnb mul transpose( delvelCorrected //Z轴有一剖分速度是抵消中立加速度的积分的 de l ve lnav. z + GRAVITY MSS*imuDataDelayed. de veldt //飞机的姿态实际描述的是从body坐标系到nav坐标系的旋转,姿态的四元素 /的余弦矩阵表示的是从body到naⅴ的旋转,那么,四元素余弦矩阵的转置矩阵就 //表示nav到body的旋转,由」是止则化的,逆矩阵和转置矩阵相同,才有如卜表 //达式,3.3版本就是先求出余弦矩阵,在转置的 stateStruct quat inverse(. rotation matrix(prevI'nb //计算速度的变化率 veldotned- delvelNav/ imuDataDelayed delveldt //一阶低通滤波 veldotnedfilt- veldotned *0. 05f veldotneDfilt *0. 95f /求出速率模的大小,gps的方差佔计有用这个值 accNavMag vel DotNEDfilt length /根据水平方向的速率大小米修正了水平方向的速度 accNavMagHoriz= norm (velDotNEDfilt. x, velDotNEDfilt. y) if ((PV AidingMode== AID none)&&(accNavMagHoriz >5. 0f))I float gain =5. 0f/ accNavMagHoriz de lvelnav x gain delve lnav y le- gain Vector 3f last Velocity= stateStruct. velocit //更新速度 statestruct. velocity + delvelnav //更新位置 stateStruct. position +=(stateStruct velocity lastVelocity)* (imuDataDelayed. delVelDT=*0. 5f) //这两个值,光流那块好像有用 de lang body of= delAng Corrected deltimeoF + imuDataDe layed de langdt //约束ekf状态 ConstrainStateso 雷神空天-植保无人机http://www.sclskt.com 2、为每个imu启动了一个ekf滤波器 Ekf是根据了mu1和imu2的状态,选择是用imu1的数据,还是用imu2的数据,还是 同时用imu1和imu2的数据;ekf2的做法是为每个imu启动一个ekf,各自维护自凵的状态 表,通过检査每个ekf滮波器的状态来进行切换,这样做的好处是一个ekf滤波器出问题了, 还有备用的,显然也是为增加的系统的稳定性,才这么设计的,之前在用ekf的时候我们也 会经常反问一个问题,如果ekf出问题了,怎么办;估计这里就是给出了这个问题的答案。 1、滤波器的初始化 bool NavEKF2 Initialise Filter(void /计算启动多少个ekf, imuMask默认设置的是3,就是启动1和2两个imu num cores=o for(uint8 t i=0 i<7; i++i if (imuMask &(1U<<i))f num cores++ /灼造了两个ek对象 core new NavEKF2 corenum cores 更新滤波器 id NavEKF2: Update Filter void) ∥/更新滤波器 e update filter state PredictEnabled[il /连续105检测到主imu不健康 if (!run Core Selection)t static uint64 t lastUnhealthy Time us=0; if (!core[primary]. healthy l lastUnhealthy Time _us ==o)f lastUnhealthy time us= imuSampletime us lection =(ime ple l ime us -las s)>1E7; ∥/计算主滤波器计算出值误差 float primary ErrorScore=coreprimary].errorScoreo