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^t
    estimat->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