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