typedef struct estimatorStruct{float pos[3];float vel[3];float acc[3];}estimator_s; /*** @brief: 根據(jù)位置加速度來計算位置和速度 Inertial filter, implementation taken from PX4 implementation by Anton Babushkin <rk3dov@gmail.com>* @param {int} axis* @param {float} dt* @param {float} acc* @return {*}*/ void inavFilterPredict(estimator_s *estimat, int axis, float dt, float acc) {estimat->pos[axis] += estimat->vel[axis] * dt + acc * dt * dt / 2.0f; //s = s + v*t + 1/2*a*t^testimat->vel[axis] += acc * dt; //v=v+a*t } /*位置校正*/ void inavFilterCorrectPos(estimator_s *estimat, int axis, float dt, float error, float weight) {float ewdt = error * weight * dt;estimat->pos[axis] += ewdt;estimat->vel[axis] += weight * ewdt; } /*速度校正*/ void inavFilterCorrectVel(estimator_s *estimat, int axis, float dt, float error, float weight) {estimat->vel[axis] += error * weight * dt; }

程序原理：

1. void inavFilterPredict(estimator_s *estimat, int axis, float dt, float acc)

通過加速度acc和時間dt的積分直接出算位移和速度，加速度計有誤差時，積分就會不斷的累計誤差，速度就不可能為0，位移也會一直往一個方向增加。

2. void inavFilterCorrectPos(estimator_s *estimat, int axis, float dt, float error, float weight)

通過另外一個位移來修正上面的位移誤差,那修正時就必須要知道誤差是多少，所以要有error，如果另外一個位移計的準(zhǔn)確度是100%，那weight就是1 ，結(jié)果加上這個error*1*dt就等于另外一個位移計了。準(zhǔn)確度是不可能100%，那weight也不要大于1。weight就是另外一個位移計的權(quán)重/可信度。位移的修正值是e*w*dt,速度的修正值是e*w*dt*dt. 誤差是不斷變化的，所以修正修值也是不斷變化的。誤差是位移的誤差。

3.void inavFilterCorrectVel(estimator_s *estimat, int axis, float dt, float error, float weight)

上面修正了位移和速度，但如果結(jié)果速度的修正還不夠的話，同理也可以再給速度單獨一個修正的權(quán)重。這時的誤差是速度的誤差，修正值是e*w*dt

總結(jié)

以上是生活随笔為你收集整理的inavFilter 惯导融合算法的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。