KF 和 EKF 实践

最新推荐文章于 2024-08-30 17:36:51 发布

机械不学习

最新推荐文章于 2024-08-30 17:36:51 发布

阅读量604

点赞数

分类专栏： slam 基础知识文章标签：数学建模

本文链接：https://blog.csdn.net/lqql2012/article/details/109027219

版权

slam 同时被 2 个专栏收录

3 篇文章 0 订阅

订阅专栏

基础知识

3 篇文章 0 订阅

订阅专栏

本文详细介绍了卡尔曼滤波及其扩展形式EKF，解释了EKF如何处理非线性问题，以及如何调整滤波参数如Q和R。同时，文章提到了UKF在输出非高斯分布情况下的局限性，并引入了粒子滤波器作为替代方案。最后，文章提及了EKF在SLAM（同时定位与建图）中的应用。

摘要由CSDN通过智能技术生成

1 KF

2 Extended Kalman Filter

2.1 边缘分布和条件分布

回顾一下高斯分布：

如果原变量为高斯分布，则边缘化和条件概率仍然满足高斯分布

则边缘分布和条件分布的模型：

2.2 KF和EKF

EKF相较于KF，就是把KF的状态转移矩阵和观测矩阵换为关于状态变量的雅可比矩阵

2.2.0 推导卡尔曼滤波

简单理解卡尔曼增益可以视为一种权重，状态估计的协方差矩阵将不停的更新适应，使得估计误差最小，其实相当于一种迭代

1）首先明确状态量和观测量是哪些，以及对应的状态转移矩阵和观测矩阵 ;

2）对于EKF需要明确，状态转移矩阵（运动模型）和观测矩阵（观测模型）都将关于状态量求导 ;

3）预测阶段：通过运动模型更新状态量，通过状态转移矩阵和状态量协方差 更新状态估计的协方差矩阵 ;

4）根据估计误差最小的原则计算卡尔曼增益 ;

得：

合并包含Kk的项目通过化简得：

参考：卡尔曼滤波详解

5）最后更新状态估计的协方差矩阵

参考：我所理解的卡尔曼滤波

2.2.1 卡尔曼滤波如何调参？

Q和R是模型误差与测量误差的大小，是模型预测值与测量值的加权,Q表征了系统模型的统计特性，增加Q中元素的值，等价于增加系统噪声或增加系统参数的不确定性， R 表征了测量噪声，增加 R 中元素的值，意味加大测量噪声的影响。举例而言，R固定，Q越大，代表越信任测量值，Q无穷代表只用测量值；反之，Q越小代表越信任模型预测值，Q为零则是只用模型预测

：q越小，越依赖系统模型，r越小，越依赖观测值

注意这里的Q和R和2.2中是相反的。。

以IMU滤波为例：

dt表示采样时间,它的值要特别精确；
Q_angle表示加速度计过程噪声协方差;
Q_bias表示陀螺仪过程噪声协方差;
R_measure表示测量噪声协方差;
P表示误差协方差矩阵;

当dt过小时 dt=0.001，滤波非常滞后; 当dt过大时 dt=0.010，明显过冲 ; dt=0.005时可以很好的跟随;

当Q_angle越大时 Q_angle=1.0 ，跟随趋势越紧密，滤波之后的噪声也越大; 缩小 Q_angle=0.01 跟随趋势变缓，滤波效果改善明显; 也就是说缩小Q_angle 时，相当于更相信预测值

P，它是误差协方差初始值，表示我们对当前预测状态的信任度，它越小说明我们越相信当前预测状态；它的值决定了初始收敛速度，一般开始设一个较小的值以便于获取较快的收敛速度。随着卡尔曼滤波的迭代，P的值会不断的改变，当系统进入稳态之后P值会收敛成一个最小的估计方差矩阵，这个时候的卡尔曼增益也是最优的，所以这个值只是影响初始收敛速度

a. 如果矩阵是个常量，不依赖于当前时间，那么就不用写下标 k。

b. 前一个系统状态估计值，英文称为prveious，即 k-1 时刻的系统状态估计值：

c. 先验状态估计值，英文称为 priori，简单讲就是对系统状态的估计值：

d. 后验状态英文称为posteriori，简单讲就是对系统的测量值即观测值：

对于我们的惯性测量模块，问题是系统状态是隐藏的，我们不能直接获取系统的角度，只能通过观测方程

1）为系统状态矩阵，大小为 n*1 列，对于MPU6050，n为2，因此可以这样表示系统状态矩阵：

其中表示角度，表示系统角速度偏差bias，基于加速度计和陀螺仪测量的角速度偏差即陀螺仪的漂移量，理论上，如果从陀螺仪的测量值减去这个偏差就可以得到陀螺仪的真实角速度值。

2）状态转移模型矩阵 F：

3）系统状态方程

其中 B 表示控制输入模型矩阵，大小为 n*k，对于我们的系统应该定义为，角度= *，因为我们不能直接通过角速度的计算得到偏差，因此，我们将矩阵 B 的底部设为0，

4）过程噪声协方差矩阵 Q

对于我们的系统那就是加速度计的状态估计协方差以及陀螺仪的偏差协方差。我们假定系统的加速度计以及陀螺仪偏差估计都是独立的，即是相互解耦的，注意它是跟时间相关的，这将造成过程噪声随着时间的增加越来越大，比如陀螺仪的漂移

5）测量值和测量噪声方差 R

其中、

6）先验状态估计值

7）先验误差协方差矩阵 P

表示我们对当前预测状态的信任度，它越小说明我们越相信当前预测状态

8）残差

其中为实际测量值，为先验状态即预测值

9）残差协方差

其中观测模型 H 用于将先验误差协方差矩阵映射到观测空间，如果不知道误差协方差矩阵P的起始值，我们可以将P设置为

，L初始以一个较大的值代替。对于我们的系统，一开始的起始角度是确定的，而且一开始的陀螺仪偏差也是可以校正的，因此我们可以认为系统的初始状态是确定的，因此我们可以初始化误差协方差矩阵为：

10）卡尔曼增益

卡尔曼增益用于表示我们对于残差的信任度，如果对于估计状态的信任度越高，误差协方差矩阵将会越小，因此卡尔曼增益将会随之变小；反之，如果我们对于残差的信任度越高，那么我们对于当前状态的预测的信任度就会降低

11）更新后验协方差矩阵

//因为偏差不能通过直接测量得到，可以认为先验状态偏差等于它前一个状态的偏差，因此只计算角速度rate和角度angle
rate = newRate - bias;
angle += dt * rate;

P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);
P[0][1] -= dt * P[1][1];
P[1][0] -= dt * P[1][1];
P[1][1] += Q_gyroBias * dt;

//经过验证，初始参数设置为以下值时适用于大多数的IMU，并且这些初始参数将会使mpu6050工作在最佳状态；
float Q_angle = 0.001;
float Q_gyroBias = 0.003;
float R_measure = 0.03;

参考：A practical approach to Kalman filter and how to implement it

深度解析卡尔曼滤波在IMU中的使用

Kalman::Kalman() {
    /* We will set the variables like so, these can also be tuned by the user */
    Q_angle = 0.001f;
    Q_bias = 0.003f;
    R_measure = 0.03f;

    angle = 0.0f; // Reset the angle
    bias = 0.0f; // Reset bias

    /*
    Since we assume that the bias is 0 and we know the starting angle (use 
    setAngle), the error covariance matrix is set like so 
    */
    P[0][0] = 0.0f; 
    P[0][1] = 0.0f;
    P[1][0] = 0.0f;
    P[1][1] = 0.0f;
};

// The angle should be in degrees and the rate should be in degrees per second and 
// the delta time in seconds
float Kalman::getAngle(float newAngle, float newRate, float dt) {

    // Update xhat - Project the state ahead
    /* Step 1 */
    rate = newRate - bias;
    angle += dt * rate;

    // Update estimation error covariance - Project the error covariance ahead
    /* Step 2 */
    P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);
    P[0][1] -= dt * P[1][1];
    P[1][0] -= dt * P[1][1];
    P[1][1] += Q_bias * dt;

    // Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
    // Calculate Kalman gain - Compute the Kalman gain
    /* Step 4 */
    float S = P[0][0] + R_measure; // Estimate error
    /* Step 5 */
    float K[2]; // Kalman gain - This is a 2x1 vector
    K[0] = P[0][0] / S;
    K[1] = P[1][0] / S;

    // Calculate angle and bias - Update estimate with measurement zk (newAngle)
    /* Step 3 */
    float y = newAngle - angle; // Angle difference
    /* Step 6 */
    angle += K[0] * y;
    bias += K[1] * y;

    // Calculate estimation error covariance - Update the error covariance
    /* Step 7 */
    float P00_temp = P[0][0];
    float P01_temp = P[0][1];

    P[0][0] -= K[0] * P00_temp;
    P[0][1] -= K[0] * P01_temp;
    P[1][0] -= K[1] * P00_temp;
    P[1][1] -= K[1] * P01_temp;

    return angle;
};

void Kalman::setAngle(float angle) { this->angle = angle; }; // Used to set angle, this should be set as the starting angle
float Kalman::getRate() { return this->rate; }; // Return the unbiased rate

/* These are used to tune the Kalman filter */
void Kalman::setQangle(float Q_angle) { this->Q_angle = Q_angle; };
void Kalman::setQbias(float Q_bias) { this->Q_bias = Q_bias; };
void Kalman::setRmeasure(float R_measure) { this->R_measure = R_measure; };

float Kalman::getQangle() { return this->Q_angle; };
float Kalman::getQbias() { return this->Q_bias; };
float Kalman::getRmeasure() { return this->R_measure; };

2.2 UKF 无迹卡尔曼

2.3 粒子滤波器

UKF的一个问题是输出仍假设成高斯分布。然而，即使在很简单的情况下，高斯的非线性变换仍然不是高斯。并且，仅在很少的情况下，输出的分布有个名字（比如卡方），多数时候你都不知道他们是啥……更别提描述它们了。
因为描述很困难，所以粒子滤波器采用了一种暴力的，用大量采样点去描述这个分布的方法（老子就是无参的你来打我呀）。框架大概像下面这个样子，就是一个不断采样——算权重——重采样的过程