inavFilter 惯导融合算法

typedef struct estimatorStruct{float pos[3];float vel[3];float acc[3];}estimator_s; /*** @brief: 根据位置加速度来计算位置和速度 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，如果另外一个位移计的准确度是100%，那weight就是1 ，结果加上这个error*1*dt就等于另外一个位移计了。准确度是不可能100%，那weight也不要大于1。weight就是另外一个位移计的权重/可信度。位移的修正值是e*w*dt,速度的修正值是e*w*dt*dt. 误差是不断变化的，所以修正修值也是不断变化的。误差是位移的误差。

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

上面修正了位移和速度，但如果结果速度的修正还不够的话，同理也可以再给速度单独一个修正的权重。这时的误差是速度的误差，修正值是e*w*dt

总结

以上是生活随笔为你收集整理的inavFilter 惯导融合算法的全部内容，希望文章能够帮你解决所遇到的问题。

如果觉得生活随笔网站内容还不错，欢迎将生活随笔推荐给好友。