ekf2在與ekf相比在思想上有了一個(gè)值得飛躍，ekf關(guān)注的是如何把數(shù)據(jù)融合進(jìn)去；ekf2通過(guò)互補(bǔ)濾波的思想，重新架構(gòu)了算法；ekf濾波在ekf中主要是用來(lái)需要在imu預(yù)測(cè)的誤差，而不是想在ekf中直接輸出姿態(tài)、速度、位置。
雷神空大-植保無(wú)人機(jī)htt://ww. sclskt. com/ 2、對(duì) readIMUData(函數(shù)進(jìn)行分析 void NavEKF2 core: readIMUData( //獲取加速度計(jì)的積分速度 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 //記錄新的數(shù)據(jù)時(shí)間 imuDataNew time ms imuSamplefime ms //累計(jì)積分時(shí)間 imuDa taDownSampledNew de langat + imuDataNew de langdt imuDataDown SampledNew de veldt + imuDataNew delveldt //累計(jì)積分角度 imuQuat Down SampleNew rotate(imuDataNew delang imuQuatDownSampleNew normalize //求陀螺儀變化的旋轉(zhuǎn)矩陣,目的是把數(shù)據(jù)的改變投影過(guò)米 Matrix3f deltarotMat imuQuatDownSampleNew. rotation matrix(deltaRotMat) //積累積分速度 / apply the delta velocity to the delta velocity accumulator imuDataDownSampledNew. delvel= deltaRotMatkimuDataNew delvel ekf間隔處理imu數(shù)據(jù),不用每次都處理,這里處理的頻率大概為50Hz,而gps的頻 率為 //5Hz,氣壓計(jì)為10hz,所有這些傳感器的數(shù)據(jù)實(shí)際是不影響的 雷神空天植保無(wú)人札http://www.sclskt.com if ((dtIMUavg*(float)framesSincePredict >=EKF TARGEt dt & startPredictEnabled)(dtIMUavg*(float)frames SincePredict > 2. 0f*EKF TARGET DT))( //通過(guò)imu在這段時(shí)間中的變化量求出旋轉(zhuǎn)的弧度大小(旋轉(zhuǎn)軸*旋轉(zhuǎn)角度) imuQuat Down SampleNew to axis angle(imuDataDown SampledNew. delAng) /記錄采樣時(shí)間 i muDataDown dNew. time ms- imuSamplcTime ms //記錄本次累計(jì)采樣數(shù)據(jù)到環(huán)形 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 //運(yùn)行狀態(tài)更新設(shè)置 runUpdates-true ∥/讀取最久的一次采樣,這個(gè)跟最新的數(shù)據(jù)相差了size(13或26)個(gè)采樣 // 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 雷神空天-植保無(wú)人機(jī)http://www.sclskt.com 3、ca| cOutputstates函數(shù)中實(shí)現(xiàn)了互補(bǔ)濾波 void NavEKF2 core:: calcOutputStateso) /0號(hào)中的代碼運(yùn)行頻率是400hz,使用im數(shù)據(jù)來(lái)預(yù)測(cè)姿態(tài)、速度、位置 //獲取陀螺儀的積分角度 Vector 3f de lAng NewCorrected =imuDataNew de lAng //獲取加速度機(jī)的積分速度 Vector 3f delve lNewCorrected imuDataNew devel //修正加速度積分的速度 correctDeltavelocity(delVelNewCorrected, imuDataNew delvelDT) //imu:積分的角度加上ekf的對(duì)現(xiàn)在角度的修正值(這里是姿態(tài)的互補(bǔ)濾波) Vector 3f delang- delAngNew Corrected delAngCorrection /將計(jì)算的三角角度轉(zhuǎn)換成四元素 Quaternion del taQuat deltaQuat. from axis angle(delang); //在前一次輸出上進(jìn)行旋轉(zhuǎn) outputDataNew quat *-deltaQuat outputDataNew. quat. normalize //求出從body坐標(biāo)系到nav坐標(biāo)系的旋轉(zhuǎn)余弦矩陣 Matrix3f Ton temp outputDataNew quat rotation matrix(Tbn temp) //把速度旋轉(zhuǎn)到nav坐標(biāo)系中 Vector 3f del ve lNav= Tbn temp*delvelNewCorrected delve lnay z + gravity MSS kimudatanew delveld //保存上一次的速度 Vector 3f lastVelocity -outputDataNew velocity //計(jì)算當(dāng)前速度 outputDataNew. velocity + delvelnav //通過(guò)開(kāi)始速度和當(dāng)前速度計(jì)算位置 雷神空天植保無(wú)人札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可補(bǔ)濾波,到這里姿態(tài)、速度、位置的預(yù)測(cè)就完成了 /互補(bǔ)濾波的實(shí)現(xiàn) f (runUpdates /把新的姿態(tài)保存到環(huán)形υ uffer,這里不是每次郗會(huì)保存的,只有運(yùn)行一次ekf 才會(huì)保存一次 storedOutput [storedIMU get youngest index]= outputDataNew //獲取最久的一次保存,時(shí)間差在0.02s以?xún)?nèi) utput DataDelaye toredOutput [stored IMU get olde dex () /求出ekf的姿態(tài)與時(shí)間最久的姿態(tài)差四元素(就是旋轉(zhuǎn)的角度用四元素表示) Quaternion quatErr stateStruct quat outputDataDelayed quat //這個(gè)地方是求四元素旋轉(zhuǎn)轉(zhuǎn)換成三角角度表示,做了個(gè)近似處理,當(dāng) theta很 的時(shí)候, theta=sin( theta)= theta/2 quatErr, normalize(; 雷神空天植保無(wú)人札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] //這里計(jì)算了一個(gè)阻尼比,計(jì)算方式也很簡(jiǎn)單,就是: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 //計(jì)算姿態(tài)的修正值,在400hz的循環(huán)里用這個(gè)值 delangCorrection=deltaAngErr *k errorGain x dtImUavg /計(jì)算速度和位置的差 Vector3f velErr =(stateStruct velocity- outputDataDelayed. velocity) Vector 3f posErr =(stateStruct position output DataDelayed position //這里通過(guò)設(shè)置的時(shí)間常數(shù)和ekr平均運(yùn)行的時(shí)間相比,求出一個(gè)增益 //用這個(gè)增益的的二次函數(shù)對(duì)速度和位置差的積分進(jìn)行了處理,得到了速度 位置修正值,這里應(yīng)該有用原始數(shù)據(jù)仿真過(guò) 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 //用了二次冪函數(shù)擬合修正值 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 雷神空天-植保無(wú)人機(jī)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 //更新向外提供的數(shù)據(jù) tputDataN [storedIMU get youngest index] 4、上面提到了ekf2的姿態(tài)、位置和速度,有必要看下姿態(tài)的融合 正常的ekf算法融合是通過(guò)測(cè)量值,求出革新值,通過(guò)革新值求增益,通過(guò)增益更新Ek 狀態(tài)和協(xié)方差,無(wú)論是ekf和ek2對(duì)應(yīng)四元素的融合思路都是用inu積分預(yù)測(cè)姿態(tài)、速度、 位置,更新協(xié)方差;用協(xié)方差更新姿態(tài)的過(guò)程在其他傳感器融合的時(shí)候順便做了,這樣做可 能是由于imu的高頻,連續(xù)性不好。如果剛興趣可以看我的“px4平臺(tái)之我見(jiàn)”,里面有介 紹融合的實(shí)現(xiàn)細(xì)節(jié)。 用imu數(shù)據(jù)預(yù)測(cè)姿態(tài)、速度、位置 UpdateStrapdownEquationsNEDO 預(yù)測(cè)協(xié)方差 Covariance Prediction(; void NavEKF2 core: UpdateStrapdownEquationsNEDO // delAng Correcte是ekf記錄的最久的角度值,大概跟最新的相差size(13/26)個(gè)數(shù)據(jù), ∥/大概相差sze*4個(gè)采樣,這樣做可以消除一些瞬時(shí)誤差,用于跟快速的姿態(tài)直接預(yù) 測(cè)方法形成互補(bǔ)濾波增加系統(tǒng)的穩(wěn)定性。 這里考慮了地球的自轉(zhuǎn),把地球自轉(zhuǎn)變化到了body坐標(biāo)系中 stateStruct. quat. rotate delAng Corrected - prevTnb earth Ratened imu Data Delayed. delAngDT) stateStruct. quat. normalize: // delvelcorrected是加速度計(jì)的積分是在body坐標(biāo)系中完成的, //需要把速度轉(zhuǎn)到nav坐標(biāo)系,才能求速度和位置。 Vector 3f de lvelnav;// delta velocity vector in earth axes 雷神空天植保無(wú)人札http://www.sclskt.com del ve lnav= prevTnb mul transpose( delvelCorrected //Z軸有一剖分速度是抵消中立加速度的積分的 de l ve lnav. z + GRAVITY MSS*imuDataDelayed. de veldt //飛機(jī)的姿態(tài)實(shí)際描述的是從body坐標(biāo)系到nav坐標(biāo)系的旋轉(zhuǎn),姿態(tài)的四元素 /的余弦矩陣表示的是從body到naⅴ的旋轉(zhuǎn),那么,四元素余弦矩陣的轉(zhuǎn)置矩陣就 //表示nav到body的旋轉(zhuǎn),由」是止則化的,逆矩陣和轉(zhuǎn)置矩陣相同,才有如卜表 //達(dá)式,3.3版本就是先求出余弦矩陣,在轉(zhuǎn)置的 stateStruct quat inverse(. rotation matrix(prevI'nb //計(jì)算速度的變化率 veldotned- delvelNav/ imuDataDelayed delveldt //一階低通濾波 veldotnedfilt- veldotned *0. 05f veldotneDfilt *0. 95f /求出速率模的大小,gps的方差佔(zhàn)計(jì)有用這個(gè)值 accNavMag vel DotNEDfilt length /根據(jù)水平方向的速率大小米修正了水平方向的速度 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) //這兩個(gè)值,光流那塊好像有用 de lang body of= delAng Corrected deltimeoF + imuDataDe layed de langdt //約束ekf狀態(tài) ConstrainStateso 雷神空天-植保無(wú)人機(jī)http://www.sclskt.com 2、為每個(gè)imu啟動(dòng)了一個(gè)ekf濾波器 Ekf是根據(jù)了mu1和imu2的狀態(tài),選擇是用imu1的數(shù)據(jù),還是用imu2的數(shù)據(jù),還是 同時(shí)用imu1和imu2的數(shù)據(jù);ekf2的做法是為每個(gè)imu啟動(dòng)一個(gè)ekf,各自維護(hù)自凵的狀態(tài) 表,通過(guò)檢査每個(gè)ekf滮波器的狀態(tài)來(lái)進(jìn)行切換,這樣做的好處是一個(gè)ekf濾波器出問(wèn)題了, 還有備用的,顯然也是為增加的系統(tǒng)的穩(wěn)定性,才這么設(shè)計(jì)的,之前在用ekf的時(shí)候我們也 會(huì)經(jīng)常反問(wèn)一個(gè)問(wèn)題,如果ekf出問(wèn)題了,怎么辦;估計(jì)這里就是給出了這個(gè)問(wèn)題的答案。 1、濾波器的初始化 bool NavEKF2 Initialise Filter(void /計(jì)算啟動(dòng)多少個(gè)ekf, imuMask默認(rèn)設(shè)置的是3,就是啟動(dòng)1和2兩個(gè)imu num cores=o for(uint8 t i=0 i<7; i++i if (imuMask &(1U<<i))f num cores++ /灼造了兩個(gè)ek對(duì)象 core new NavEKF2 corenum cores 更新濾波器 id NavEKF2: Update Filter void) ∥/更新濾波器 e update filter state PredictEnabled[il /連續(xù)105檢測(cè)到主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; ∥/計(jì)算主濾波器計(jì)算出值誤差 float primary ErrorScore=coreprimary].errorScoreo