从 EKF 到粒子滤波：定位技术的奇妙之旅与 QT 仿真实现

ekf 扩展卡尔曼滤波定位 qt仿真程序粒子滤波定位

在机器人定位与导航的领域中，扩展卡尔曼滤波（EKF）和粒子滤波是两种极为重要的算法，它们各有千秋，在不同场景下发挥着关键作用。今天咱就结合 QT 仿真程序，唠唠这俩算法在定位中的应用。

EKF 扩展卡尔曼滤波定位

EKF 原理

EKF 是对传统卡尔曼滤波的一种扩展，主要用于处理非线性系统。它的核心思路是通过对非线性系统在当前估计值处进行一阶泰勒展开，将非线性问题近似线性化，然后套用卡尔曼滤波的框架进行处理。

QT 仿真代码示例

// 假设我们有一个简单的非线性系统模型 f(x) = x^2 + 1 // 状态转移函数 Eigen::VectorXd f(const Eigen::VectorXd& x) { Eigen::VectorXd result(1); result(0) = x(0) * x(0) + 1; return result; } // 观测函数 Eigen::VectorXd h(const Eigen::VectorXd& x) { Eigen::VectorXd result(1); result(0) = x(0); return result; } // EKF 实现 void EKF() { Eigen::VectorXd x_hat(1); // 估计状态 Eigen::MatrixXd P(1, 1); // 估计协方差 // 初始化 x_hat << 0; P << 1; Eigen::MatrixXd Q(1, 1); // 过程噪声协方差 Eigen::MatrixXd R(1, 1); // 观测噪声协方差 Q << 0.01; R << 0.1; // 模拟观测 Eigen::VectorXd z(1); z << 1.5; // 预测步骤 Eigen::VectorXd x_hat_minus = f(x_hat); Eigen::MatrixXd F(1, 1); F << 2 * x_hat(0); // 状态转移函数的雅可比矩阵 Eigen::MatrixXd P_minus = F * P * F.transpose() + Q; // 更新步骤 Eigen::MatrixXd H(1, 1); H << 1; // 观测函数的雅可比矩阵 Eigen::MatrixXd K = P_minus * H.transpose() * (H * P_minus * H.transpose() + R).inverse(); x_hat = x_hat_minus + K * (z - h(x_hat_minus)); P = (Eigen::MatrixXd::Identity(1, 1) - K * H) * P_minus; }

代码分析

状态转移函数f：这里定义了一个简单的非线性函数f(x) = x^2 + 1，实际应用中这个函数会更复杂，比如机器人运动模型。
观测函数h：当前例子里它简单返回状态值，实际可能涉及传感器模型。
EKF 实现部分：初始化估计状态x_hat和协方差P，设置过程噪声Q和观测噪声R。预测步骤通过状态转移函数和雅可比矩阵更新估计状态和协方差，更新步骤结合观测值进一步优化估计。

粒子滤波定位

粒子滤波原理

粒子滤波基于蒙特卡洛方法，通过大量粒子来表示系统状态的概率分布。每个粒子带有权重，权重反映了该粒子与观测值的匹配程度。随着新观测值的到来，粒子权重不断调整，粒子分布也逐渐逼近真实状态分布。

QT 仿真代码示例

// 粒子结构体 struct Particle { Eigen::VectorXd state; double weight; }; // 粒子滤波实现 void ParticleFilter() { int num_particles = 1000; std::vector<Particle> particles(num_particles); // 初始化粒子 for (auto& particle : particles) { particle.state = Eigen::VectorXd::Random(1); particle.weight = 1.0 / num_particles; } // 模拟观测 Eigen::VectorXd z(1); z << 1.5; // 重要性采样 for (auto& particle : particles) { // 简单的状态转移 particle.state += Eigen::VectorXd::Random(1) * 0.1; // 计算权重 particle.weight *= exp(-(particle.state - z).squaredNorm() / 0.1); } // 权重归一化 double total_weight = 0; for (const auto& particle : particles) { total_weight += particle.weight; } for (auto& particle : particles) { particle.weight /= total_weight; } // 重采样 std::vector<Particle> new_particles(num_particles); std::vector<double> cdf(num_particles); cdf[0] = particles[0].weight; for (int i = 1; i < num_particles; ++i) { cdf[i] = cdf[i - 1] + particles[i].weight; } for (int i = 0; i < num_particles; ++i) { double u = (double)rand() / RAND_MAX; int index = 0; while (u > cdf[index]) { index++; } new_particles[i] = particles[index]; } particles = new_particles; }