Px4
速度位置
kalman
滤波算法分析
1
、速度和位置的融合为什么用
Kalman Filter
,二不用
ekf
前两节分析的空速传感器和磁力计,用的是 Extended Kalman Filter ,而本节分析的速
度位置的融合所用的是 Kalman Filter。怎么这么说呢?求 Extended Kalman Filter 的思路是用
泰勒级数展开,取一阶偏导(高阶偏导舍去),按照 Kalman Filter 的思路求解。前两节都求
了雅克比偏导数,就是泰勒级数展开的一阶偏导。
速度和位置的融合的传感器是 gps、气压计和光流传感器,它们测试的值直接是各个方
向的速度和位置,正好满足 Kalman Filter 的三个前提条件:
当前状态的概率分布必须是上一状态和将要执行的控制量的线性函数,再叠加一个高斯
噪声;
对状态的测量必须是状态的线性函数叠加高斯噪声;
初始状态分布为高斯分布;
2
、
Kalman Filter
的
34
维状态变量:
struct state_elements {
Quaternion quat; // 0..3 四元素
Vector3f velocity; // 4..6 速度值
Vector3f position; // 7..9 位置值
Vector3f gyro_bias; // 10..12 陀螺仪的值
float accel_zbias1; // 13 加速度计 1 Z 轴的值
Vector2f wind_vel; // 14..15 风的速度
Vector3f earth_magfield; // 16..18 地磁场值
Vector3f body_magfield; // 19..21 机体磁场值
float accel_zbias2; // 22 加速度计 2 Z 轴的值
Vector3f vel1; // 23 .. 25 IMU1 加速度计积分的速度
float posD1; // 26 IMU1 加速度计积分的高度
Vector3f vel2; // 27 .. 29 IMU2 加速度计积分的速度
float posD2; // 30 IMU2 计速度计积分的高度
Vector3f omega; // 31 .. 33
} &state;
State 34 个状态中跟速度和位置相关的有 velocity、position、accel_zbias1、accel_zbias2、vel1、
posD1、vel2、posD2。
3、Kalman filter 的五条黄金公式
评论0