当前位置： 首页 > news >正文

slam典型应用手搓

news 来源：原创 2024/9/28 17:29:47

一.雅可比矩阵的计算

假设我们有一个三维状态向量 [x, y, z],并且定义一个新的观测模型。输出两个观测值[u, v],其中

u = x^2 + y^2 + z, v = sin(x) + cos(y) + e^z，演示雅可比矩阵的计算

#include <iostream>
#include <Eigen/Dense>
#include <cmath>/*
我们可以使用一个更复杂的例子来演示雅可比矩阵的计算。假设我们有一个三维状态向量 [x, y, z],并且定义一个新的观测模型。输出两个观测值[u, v],其中
u = x^2 + y^2 + z, v = sin(x) + cos(y) + e^z
*/// 状态向量:[x, y, theta]
Eigen::Vector3d state; // 观测模型
Eigen::Vector2d observationModel(const Eigen::Vector3d& state){// 假设观测为一个3D点double x = state[0];double y = state[1];double z = state[2];double u = x * x + y * y + z;double v = std::sin(x) + std::cos(y) + std::exp(z);return Eigen::Vector2d(x, y);
}// 计算雅可比矩阵
Eigen::Matrix<double, 2 ,3> computeJacobian(const Eigen::Vector3d& state){Eigen::Matrix<double, 2, 3> J;double x = state[0];double y = state[1];double z = state[2];// 对于x和y的偏导数J(0, 0) = 2 * x;    // u对x的偏导J(0, 1) = 2 * y;    // u对y的偏导J(0, 2) = 1;        // u对z的偏导J(1, 0) = std::cos(x);     // v对x的偏导J(1, 1) = -std::sin(y);    // v对y的偏导J(1, 2) = std::exp(z);     // v对z的偏导return J;
}int main(int argc, char *argv[])
{// 示例状态state << 2, 3, 0;         // x = 2, y = 3, theta = 0// 计算观测Eigen::Vector2d z = observationModel(state);std::cout << "观测Observation: " << z.transpose() << std::endl;// 计算雅可比矩阵Eigen::Matrix<double, 2, 3> J = computeJacobian(state);std::cout << "Jaconbian Matrix :\n" << J <<std::endl;return 0;
}

二.里程计

编写一个运动模型的实现，如里程计模型，给定控制输入预测下一个状态

#include <iostream>
#include <Eigen/Dense>/*
编写一个运动模型的实现，如里程计模型，给定控制输入预测下一个状态
*/// 状态向量[x, y, theta]
Eigen::Vector3d state;// 运动模型
Eigen::Vector3d motionModel(const Eigen::Vector3d& state, double v, double omega, double dt){double x = state[0];double y = state[1];double theta = state[2];// 更新状态double newX = x + v * std::cos(theta) * dt;double newY = y + v * std::sin(theta) * dt;double newTheta = theta + omega * dt;// 返回新的状态return Eigen::Vector3d(newX, newY, newTheta);
}int main(){// 初始状态state << 0, 0, 0;// 控制输入:线速度和角速度double v = 1.0;        // 1.0m/sdouble omega = 0.1;    // 0.1rad/sdouble dt = 1.0;       // 时间间隔 1s// 计算新的状态Eigen::Vector3d newState = motionModel(state, v, omega, dt);std::cout << "新的状态: " << newState.transpose() << std::endl;return 0;
}

三.卡尔曼滤波

实现一个二维卡尔曼滤波器，使用二维状态向量，假设我们要跟踪物体在平面上的位置和速度。状态向量 [x, y, vx, vy].包括位置(x, y),速度(vx, vy)

#include <iostream>
#include <Eigen/Dense>/*
实现一个二维卡尔曼滤波器，使用二维状态向量，假设我们要跟踪物体在平面上的位置和速度。状态向量 [x, y, vx, vy].包括位置(x, y),速度(vx, vy)
*/class kamlanFilter{
public:kamlanFilter(){// 初始化状态向量[x, y, v_x, v_y]state = Eigen::Vector4d(0, 0, 0, 0);// 初始化协方差矩阵covariance = Eigen::Matrix4d::Identity();// 状态转移矩阵F = Eigen::Matrix4d::Identity();F(0, 2) = 1;   //x = x + v_x * dtF(1, 3) = 1;   //y = y + v_y * dt// 观测矩阵H = Eigen::Matrix<double, 2, 4>::Zero();H(0, 0) = 1;    // 观测xH(1, 1) = 1;    // 观测y// 状态噪声协方差Q = Eigen::Matrix4d::Identity() * 0.1;// 观测噪声协方差R = Eigen::Matrix2d::Identity() * 1;}void predict(){// 状态预测state = F * state;// 协方差预测covariance = F * covariance * F.transpose() + Q;}void update(const Eigen::Vector2d& measurement){// 计算创新Eigen::Vector2d y = measurement - H * state;// 计算协方差Eigen::Matrix2d S = H * covariance * H.transpose() + R;// 计算卡尔曼增益Eigen::Matrix<double, 4, 2> K = covariance * H.transpose() * S.inverse();// 更新状态state += K * y;// 更新协方差covariance = (Eigen::Matrix4d::Identity() - K * H) * covariance;}Eigen::Vector4d getState() const{return state;}private:Eigen::Vector4d state;              // 状态向量Eigen::Matrix4d covariance;         // 协方差矩阵Eigen::Matrix4d F;                  // 状态转移矩阵Eigen::Matrix<double, 2, 4> H;      // 观测矩阵Eigen::Matrix4d Q;                  // 状态噪声协方差Eigen::Matrix2d R;                  // 观测噪声协方差
};int main(){kamlanFilter kf;// 模拟数据Eigen::Vector2d measurements;for(int i = 0; i < 10; i++){// 模拟测量量(假设真实值是线性运动，添加噪声)measurements(0) = i + ((double) rand() / RAND_MAX) * 0.5;   // 真实x添加噪声measurements(1) = i + ((double) rand() / RAND_MAX) * 0.5;   // 真实y添加噪声// 预测kf.predict();// 更新kf.update(measurements);std::cout << "当前状态估计: " << kf.getState().transpose() << std::endl;}return 0;
}

四.扩展卡尔曼滤波

要在现有的二维卡尔曼滤波器基础上实现扩展卡尔曼滤波（EKF），你需要以下几个步骤：

非线性状态转移模型：定义一个非线性状态转移函数。
非线性观测模型：定义一个非线性观测函数。
计算雅可比矩阵：在预测和更新步骤中，计算状态转移和观测的雅可比矩阵。
下面是修改后的代码示例，添加了扩展卡尔曼滤波的实现：

#include <iostream>
#include <Eigen/Dense>/*
要在现有的二维卡尔曼滤波器基础上实现扩展卡尔曼滤波（EKF），你需要以下几个步骤：非线性状态转移模型：定义一个非线性状态转移函数。
非线性观测模型：定义一个非线性观测函数。
计算雅可比矩阵：在预测和更新步骤中，计算状态转移和观测的雅可比矩阵。
下面是修改后的代码示例，添加了扩展卡尔曼滤波的实现：
*/class ExtendedKalmanFilter {
public:ExtendedKalmanFilter() {// 初始化状态向量 [x, y, v_x, v_y]state = Eigen::Vector4d(0, 0, 0, 0);// 初始化协方差矩阵covariance = Eigen::Matrix4d::Identity();// 状态噪声协方差Q = Eigen::Matrix4d::Identity() * 0.1;// 观测噪声协方差R = Eigen::Matrix2d::Identity() * 1;}void predict(double dt) {// 使用非线性状态转移模型Eigen::Vector4d newState;newState(0) = state(0) + state(2) * dt; // x = x + v_x * dtnewState(1) = state(1) + state(3) * dt; // y = y + v_y * dtnewState(2) = state(2); // 速度保持不变newState(3) = state(3); // 速度保持不变// 更新状态state = newState;// 计算雅可比矩阵 FF = Eigen::Matrix4d::Identity();F(0, 2) = dt; // ∂x/∂v_xF(1, 3) = dt; // ∂y/∂v_y// 协方差预测covariance = F * covariance * F.transpose() + Q;}void update(const Eigen::Vector2d& measurement) {// 使用非线性观测模型Eigen::Vector2d z_pred;z_pred(0) = state(0); // 观测 xz_pred(1) = state(1); // 观测 y// 计算创新Eigen::Vector2d y = measurement - z_pred;// 计算雅可比矩阵 HH = Eigen::Matrix<double, 2, 4>::Zero();H(0, 0) = 1; // ∂z/∂xH(1, 1) = 1; // ∂z/∂y// 计算协方差Eigen::Matrix2d S = H * covariance * H.transpose() + R;// 计算卡尔曼增益Eigen::Matrix<double, 4, 2> K = covariance * H.transpose() * S.inverse();// 更新状态state += K * y;// 更新协方差covariance = (Eigen::Matrix4d::Identity() - K * H) * covariance;}Eigen::Vector4d getState() const {return state;}private:Eigen::Vector4d state;              // 状态向量Eigen::Matrix4d covariance;         // 协方差矩阵Eigen::Matrix4d F;                  // 状态转移雅可比矩阵Eigen::Matrix<double, 2, 4> H;      // 观测雅可比矩阵Eigen::Matrix4d Q;                  // 状态噪声协方差Eigen::Matrix2d R;                  // 观测噪声协方差
};int main() {ExtendedKalmanFilter ekf;// 模拟数据Eigen::Vector2d measurements;double dt = 1.0; // 时间步长for (int i = 0; i < 10; i++) {// 模拟测量量（假设真实值是线性运动，添加噪声）measurements(0) = i + ((double)rand() / RAND_MAX) * 0.5; // 真实 x 加噪声measurements(1) = i + ((double)rand() / RAND_MAX) * 0.5; // 真实 y 加噪声// 预测ekf.predict(dt);// 更新ekf.update(measurements);std::cout << "当前状态估计: " << ekf.getState().transpose() << std::endl;}return 0;
}

五.g2o

使用g2o优化曲线

#include <iostream>
#include <g2o/core/g2o_core_api.h>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <Eigen/Core>
#include <opencv2/core/core.hpp>
#include <cmath>
#include <chrono>using namespace std;// 曲线模型的顶点，模板参数：优化变量维度和数据类型
class CurveFittingVertex : public g2o::BaseVertex<3, Eigen::Vector3d> {
public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW //这是一个宏，用于在Eigen库中创建对齐的new操作符。这可以确保在分配内存时，对象的大小与编译器指定的对齐要求相匹配// 重置virtual void setToOriginImpl() override {    //用于将顶点的位置重置为原点_estimate << 0, 0, 0;}// 更新virtual void oplusImpl(const double *update) override {    //用于更新顶点的位置_estimate += Eigen::Vector3d(update);}// 存盘和读盘：留空virtual bool read(istream &in) {}virtual bool write(ostream &out) const {}
};// 误差模型 模板参数：观测值维度，类型，连接顶点类型
class CurveFittingEdge : public g2o::BaseUnaryEdge<1, double, CurveFittingVertex> {
public:EIGEN_MAKE_ALIGNED_OPERATOR_NEWCurveFittingEdge(double x) : BaseUnaryEdge(), _x(x) {}// 计算曲线模型误差virtual void computeError() override {    //用于计算曲线模型的误差const CurveFittingVertex *v = static_cast<const CurveFittingVertex *> (_vertices[0]);const Eigen::Vector3d abc = v->estimate();_error(0, 0) = _measurement - std::exp(abc(0, 0) * _x * _x + abc(1, 0) * _x + abc(2, 0));}// 计算雅可比矩阵virtual void linearizeOplus() override {      //用于计算雅可比矩阵const CurveFittingVertex *v = static_cast<const CurveFittingVertex *> (_vertices[0]);const Eigen::Vector3d abc = v->estimate();double y = exp(abc[0] * _x * _x + abc[1] * _x + abc[2]);_jacobianOplusXi[0] = -_x * _x * y;_jacobianOplusXi[1] = -_x * y;_jacobianOplusXi[2] = -y;}virtual bool read(istream &in) {}virtual bool write(ostream &out) const {}public:double _x;  // x 值， y 值为 _measurement
};int main(int argc, char **argv) {double ar = 1.0, br = 2.0, cr = 1.0;         // 真实参数值double ae = 2.0, be = -1.0, ce = 5.0;        // 估计参数值int N = 200;                                 // 数据点double w_sigma = 1.0;                        // 噪声Sigma值double inv_sigma = 1.0 / w_sigma;cv::RNG rng;                                 // OpenCV随机数产生器vector<double> x_data, y_data;      // 数据for (int i = 0; i < N; i++) {double x = i / 100.0;x_data.push_back(x);y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));}// 构建图优化，先设定g2otypedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> BlockSolverType;  // 每个误差项优化变量维度为3，误差值维度为1typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型// 梯度下降方法，可以从GN, LM, DogLeg 中选auto solver = new g2o::OptimizationAlgorithmGaussNewton(g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));g2o::SparseOptimizer optimizer;     // 图模型optimizer.setAlgorithm(solver);   // 设置求解器optimizer.setVerbose(true);       // 打开调试输出// 往图中增加顶点CurveFittingVertex *v = new CurveFittingVertex();v->setEstimate(Eigen::Vector3d(ae, be, ce));v->setId(0);optimizer.addVertex(v);// 往图中增加边for (int i = 0; i < N; i++) {CurveFittingEdge *edge = new CurveFittingEdge(x_data[i]);edge->setId(i);edge->setVertex(0, v);                // 设置连接的顶点edge->setMeasurement(y_data[i]);      // 观测数值edge->setInformation(Eigen::Matrix<double, 1, 1>::Identity() * 1 / (w_sigma * w_sigma)); // 信息矩阵：协方差矩阵之逆optimizer.addEdge(edge);}// 执行优化cout << "start optimization" << endl;chrono::steady_clock::time_point t1 = chrono::steady_clock::now();optimizer.initializeOptimization();optimizer.optimize(10);chrono::steady_clock::time_point t2 = chrono::steady_clock::now();chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);cout << "solve time cost = " << time_used.count() << " seconds. " << endl;// 输出优化值Eigen::Vector3d abc_estimate = v->estimate();cout << "estimated model: " << abc_estimate.transpose() << endl;return 0;
}

相关文章：

  • 暴雨讲堂：算力高速互联催化超节点开启AI新篇章
  • Python知识点：如何使用Python进行无人机数据处理
  • Gstreamer中，使用mp4或者flv作为视频源去推流RTP等视频流时，需要先解码在编码才能正常
  • uniapp view设置当前view之外的点击事件
  • 类与对象—python
  • Anaconda教程
  • Kubernetes服务发布基础
  • LeetCode 149. 直线上最多的点数
  • LaTeX 编辑器-TeXstudio
  • 【计算机网络最全知识点问答】第二章 物理层
  • gitlab-runner集成CI/CD完整项目部署
  • 凤凰模拟器V6中无人机如何设置“有头模式”
  • 科研绘图系列：R语言多个AUC曲线图（multiple AUC curves）
  • Linux终端简介
  • WordPress LearnPress插件 SQL注入复现(CVE-2024-8522)
  • 深入了解以太坊
  • [译]前端离线指南（上）
  • CentOS 7 防火墙操作
  • ECMAScript6(0)：ES6简明参考手册
  • HTML-表单
  • js继承的实现方法
  • Node项目之评分系统（二）- 数据库设计
  • OSS Web直传 （文件图片）
  • PaddlePaddle-GitHub的正确打开姿势
  • PV统计优化设计
  • PyCharm搭建GO开发环境(GO语言学习第1课)
  • Python学习笔记 字符串拼接
  • SpiderData 2019年2月25日 DApp数据排行榜
  • SQLServer之创建显式事务
  • 百度贴吧爬虫node+vue baidu_tieba_crawler
  • 服务器从安装到部署全过程（二）
  • 给github项目添加CI badge
  • 基于Android乐音识别(2)
  • 理解 C# 泛型接口中的协变与逆变（抗变）
  • 那些年我们用过的显示性能指标
  • 跳前端坑前，先看看这个！！
  • 异常机制详解
  • 最近的计划
  • linux 淘宝开源监控工具tsar
  • ​一文看懂数据清洗：缺失值、异常值和重复值的处理
  • ！！java web学习笔记(一到五)
  • # 计算机视觉入门
  • #LLM入门|Prompt#1.7_文本拓展_Expanding
  • #预处理和函数的对比以及条件编译
  • （13）DroneCAN 适配器节点（一）
  • (3)Dubbo启动时qos-server can not bind localhost22222错误解决
  • （4.10~4.16）
  • (delphi11最新学习资料) Object Pascal 学习笔记---第13章第1节 （全局数据、栈和堆）
  • （javaweb）Http协议
  • （附源码）ssm本科教学合格评估管理系统 毕业设计 180916
  • （附源码）ssm基于jsp的在线点餐系统 毕业设计 111016
  • （学习日记）2024.01.19
  • (原创)攻击方式学习之(4) - 拒绝服务(DOS/DDOS/DRDOS)
  • （转）EOS中账户、钱包和密钥的关系
  • (转)Linux NTP配置详解 (Network Time Protocol)