ekf

背景

• 卡尔曼滤波（KF）是一种强大的线性滤波算法，适用于线性系统，即系统的状态转移方程和观测方程均为线性。然而，在实际应用中，许多系统是非线性的，例如机器人导航、飞行器姿态估计等。为了将卡尔曼滤波应用于非线性系统，扩展卡尔曼滤波应运而生。
• EKF通过在每个时间步对非线性系统进行线性化，从而将非线性问题转化为线性问题，再利用卡尔曼滤波的框架进行状态估计。

数学模型

EKF处理的非线性系统可以用以下形式表示：

状态转移方程

x_k​=f(x_{k−1}​, u_{k−1}​)+w_k−1​

• x_k是系统在时刻 k 的状态向量。
• f(⋅)是非线性状态转移函数。
• u_{k−1}是控制输入。
• w_{k−1}​是过程噪声，通常假设为零均值高斯噪声。

观测方程

z_k​=h(x_k​)+v_k​

• z_k​是在时刻 k 的观测向量。
• h(⋅)是非线性观测函数。
• v_k​是观测噪声，通常假设为零均值高斯噪声。

求解过程

EKF的核心思想是在每个时间步对非线性函数f(⋅)和h(⋅)进行一阶泰勒展开，从而将非线性问题近似为线性问题，然后利用卡尔曼滤波的递推公式进行状态估计。EKF的递推过程分为两个主要阶段：时间更新（预测）和测量更新（校正）。

1. 线性化

• 状态转移方程的线性化：在当前估计的状态 \hat{x}_{k−1​} 处对f(⋅)进行一阶泰勒展开: f(x_{k−1}​,u_{k−1}​)≈f(\hat{x}_{k−1}​,u_{k−1}​)+F_{k−1}​(x_{k−1}​−\hat{x}_{k−1​})其中，F_{k−1}是f(⋅)在\hat{x}_{k−1}处的雅可比矩阵:F_{k−1​}=\frac{\partial f }{\partial x}|_{\hat{x}_{k−1}​,u_{k−1}​​}
• 观测方程的线性化：在当前估计的状态\hat{x}_k​处对h(⋅)进行一阶泰勒展开:h(x_k​)≈h(\hat{x}_k​)+H_k​(x_k​−\hat{x}_k​)其中，H_k​ 是 h(⋅) 在 \hat{x}_k​ 处的雅可比矩阵： H_k​=\frac{\partial h }{\partial x}|_{\hat{x}_k​​}​​

2. 时间更新（预测）

• 状态预测： \hat{x}_{k|k−1}​=f(\hat{x}_{k−1∣k−1}​,u_{k−1​}) 这一步利用非线性函数 f(⋅) 和上一时刻的状态估计值 \hat{x}_{k−1∣k−1}​ 来预测当前时刻的状态。
• 协方差预测**： P_{k∣k−1}​=F_{k−1}​P_{k−1∣k−1​}F_{k−1}^T​+Q_{k−1}​ 其中，P_{k∣k−1}​ 是状态估计的协方差矩阵, Q_{k−1}​ 是过程噪声的协方差矩阵。

3. 测量更新（校正）

• 卡尔曼增益计算： K_k​=P_{k∣k−1​}H_k^T​(H_k​P_{k∣k−1}​H_k^T​+R_k​)^{−1} 其中,R_k​是观测噪声的协方差矩阵。
• 状态更新**： \hat{x}_{k∣k}​=\hat{x}_{k∣k−1}​+K_k​(z_k​−h(\hat{x}_{k∣k−1​})) 这一步利用观测值 z_k​ 和预测的观测值 h(\hat{x}_{k∣k−1}​) 之间的残差来校正状态估计。
• 协方差更新**： P_{k∣k}​=(I−K_k​H_k​)P_{k∣k−1​}

优点

• EKF是最早被广泛使用的非线性滤波方法之一，具有理论基础扎实、实现相对简单的特点。
• 在非线性程度较弱的情况下，EKF能够提供较好的估计精度。

缺点

• EKF依赖于非线性函数的线性化，当系统非线性较强时，线性化误差可能导致估计精度下降，甚至发散。
• 雅可比矩阵的计算较为复杂，且需要在每个时间步重新计算，增加了计算负担。
• 对初始状态估计和噪声协方差矩阵的选取较为敏感，不合理的选取可能导致滤波性能下降。

示例

输入是加速度acc(x,y,z)、角速度ang(x,y,z)，输出为位置pos(x,y,z)、速度vel(x,y,z),姿态q(w,x,y,z)

#include <iostream>
#include <Eigen/Dense>
#include <unsupported/Eigen/Quaternion>

using namespace Eigen;

class EKF {
public:
    EKF(double dt) : dt_(dt) {
        // 初始化状态向量
        state_ = VectorXd::Zero(10); // 3位置 + 3速度 + 4四元数
        state_.segment<4>(6).setConstant(1.0); // 初始化四元数为单位四元数
        // 初始化协方差矩阵
        covariance_ = MatrixXd::Identity(10, 10) * 10.0;
        // 初始化噪声协方差矩阵
        process_noise_cov_ = MatrixXd::Identity(10, 10) * 0.1;
        measurement_noise_cov_ = MatrixXd::Identity(10, 10) * 1.0;
    }

    // 预测步骤
    void predict(const Vector3d& acc, const Vector3d& ang) {
        // 提取状态向量
        Vector3d pos = state_.segment<3>(0);
        Vector3d vel = state_.segment<3>(3);
        Quaterniond q(state_.segment<4>(6));

        // 更新位置和速度
        pos += vel * dt_;
        vel += acc * dt_;

        // 更新姿态（四元数）
        Vector3d ang_dt = ang * dt_;
        double norm_ang_dt = ang_dt.norm();
        Quaterniond dq;
        if (norm_ang_dt > 0) {
            dq = Quaterniond(
                cos(norm_ang_dt / 2),
                (ang_dt / norm_ang_dt).x() * sin(norm_ang_dt / 2),
                (ang_dt / norm_ang_dt).y() * sin(norm_ang_dt / 2),
                (ang_dt / norm_ang_dt).z() * sin(norm_ang_dt / 2)
            );
        } else {
            dq = Quaterniond(1, 0, 0, 0);
        }
        q = q * dq;
        q.normalize();

        // 更新状态向量
        state_.segment<3>(0) = pos;
        state_.segment<3>(3) = vel;
        state_.segment<4>(6) = q.coeffs();

        // 更新协方差矩阵
        MatrixXd F = MatrixXd::Identity(10, 10);
        F.block<3, 3>(0, 3) = Matrix3d::Identity() * dt_;
        F.block<3, 3>(3, 3) = Matrix3d::Identity() * dt_;
        covariance_ = F * covariance_ * F.transpose() + process_noise_cov_;
    }

    // 更新步骤
    void update(const VectorXd& measurement) {
        MatrixXd H = MatrixXd::Identity(10, 10);
        MatrixXd S = H * covariance_ * H.transpose() + measurement_noise_cov_;
        MatrixXd K = covariance_ * H.transpose() * S.inverse();
        state_ = state_ + K * (measurement - state_);
        covariance_ = (MatrixXd::Identity(10, 10) - K * H) * covariance_;
    }

    VectorXd getState() const {
        return state_;
    }

private:
    double dt_; // 时间步长
    VectorXd state_; // 状态向量 [pos, vel, q]
    MatrixXd covariance_; // 状态协方差矩阵
    MatrixXd process_noise_cov_; // 过程噪声协方差矩阵
    MatrixXd measurement_noise_cov_; // 观测噪声协方差矩阵
};

int main() {
    double dt = 0.1; // 时间步长为0.1秒
    EKF ekf(dt);

    for (int i = 0; i < 10; ++i) {
        // 模拟输入：加速度和角速度
        Vector3d acc(0.1, 0.2, 0.3); // 加速度
        Vector3d ang(0.01, 0.02, 0.03); // 角速度

        // 预测步骤
        ekf.predict(acc, ang);

        // 模拟测量：位置、速度和姿态（带噪声）
        VectorXd measurement = ekf.getState() + VectorXd::Random(10) * 0.1;

        // 更新步骤
        ekf.update(measurement);

        // 输出当前估计的状态
        VectorXd state = ekf.getState();
        std::cout << "Step " << i << ":\n";
        std::cout << "Position: " << state.segment<3>(0).transpose() << "\n";
        std::cout << "Velocity: " << state.segment<3>(3).transpose() << "\n";
        std::cout << "Quaternion: " << state.segment<4>(6).transpose() << "\n\n";
    }

    return 0;
}