【李群李代数】【manif 】基于固定信标的2D机器人定位 (Error State Kalman Filter)...

demo演示

8c30742758c21c75a11b6a3f8fe42301.png

运行结果

我们考虑一个机器人在平面上被少量的准时地标或_信标 包围。

机器人以轴向速度和角速度的形式接收控制动作,并且能够测量信标相对于其自身参考系的位置。

机器人位姿 X 在 SE(2) 中,信标位置 b_k 在 R^2 中,

        | cos th  -sin th  x |

*      X = | sin th   cos th   y | //位置和方向

                    |  0     0   1 |

*     b_k = (bx_k, by_k)           // 世界坐标系中的lmk坐标 

控制信号 u se(2) 中的旋量,包括纵向速度 v 和角速度 w,没有横向速度分量,在采样时间 dt 上积分。

*      u = (v*dt, 0, w*dt)

控制被带有协方差的加性高斯噪声 u_noise 破坏

*    Q = diagonal(sigma_v^2, sigma_s^2, sigma_w^2).

此噪声解释了通过 sigma_s 非零值可能出现的横向滑移 u_s

*当控制 u 到达时,机器人位姿更新为 X <-- X * Exp(u) = X + u

地标测量是范围和方位类型,但为了简单起见,它们采用笛卡尔形式。

它们的噪声 n 是零均值高斯分布,并用协方差矩阵 R 指定。

我们注意到刚性运动动作 y = h(X,b) = X^-1 * b

*     y_k = (brx_k, bry_k)       // 机器人坐标系中的lmk坐标

 我们考虑位于已知位置的信标 b_k

我们将要估计的位姿定义为 SE(2) 中的 X

估计误差 dx 及其协方差 P X 处的切线空间中表示。

*所有这些变量再次总结如下

* * X : 机器人位姿,SE(2)

* u :机器人控制量,(v*dt ; 0 ; w*dt) in se(2)

* Q : 控制扰动协方差

* b_k : k 个地标位置,R^2

* y :机器人坐标系中的笛卡尔地标测量,R^2

* R : 测量噪声的协方差

*  The motion and measurement models are运动和测量模型是

*   X_(t+1) = f(X_t, u) = X_t * Exp ( w )    //运动方程 

*   y_k     = h(X, b_k) = X^-1 * b_k          //测量方程

下面的算法首先包括一个模拟器来产生测量结果,然后使用这些测量结果来估计状态,使用基于李的误差状态卡尔曼滤波器。最后,打印模拟状态和估计状态以及未过滤状态(即没有卡尔曼校正)可以评估估计的质量。

#include "manif/SE2.h"#include <vector>#include <iostream>
#include <iomanip>using std::cout;
using std::endl;using namespace Eigen;typedef Array<double, 2, 1> Array2d;
typedef Array<double, 3, 1> Array3d;int main()
{std::srand((unsigned int) time(0));// START CONFIGURATION////const int NUMBER_OF_LMKS_TO_MEASURE = 3;// Define the robot pose element and its covariancemanif::SE2d X, X_simulation, X_unfiltered;Matrix3d    P;X_simulation.setIdentity();X.setIdentity();X_unfiltered.setIdentity();P.setZero();// Define a control vector and its noise and covariancemanif::SE2Tangentd  u_simu, u_est, u_unfilt;Vector3d            u_nom, u_noisy, u_noise;Array3d             u_sigmas;Matrix3d            U;u_nom    << 0.1, 0.0, 0.05;u_sigmas << 0.1, 0.1, 0.1;U        = (u_sigmas * u_sigmas).matrix().asDiagonal();// Declare the Jacobians of the motion wrt robot and controlmanif::SE2d::Jacobian J_x, J_u;// Define three landmarks in R^2Eigen::Vector2d b0, b1, b2, b;b0 << 2.0,  0.0;b1 << 2.0,  1.0;b2 << 2.0, -1.0;std::vector<Eigen::Vector2d> landmarks;landmarks.push_back(b0);landmarks.push_back(b1);landmarks.push_back(b2);// Define the beacon's measurementsVector2d                y, y_noise;Array2d                 y_sigmas;Matrix2d                R;std::vector<Vector2d>   measurements(landmarks.size());y_sigmas << 0.01, 0.01;R        = (y_sigmas * y_sigmas).matrix().asDiagonal();// Declare the Jacobian of the measurements wrt the robot poseMatrix<double, 2, 3>    H;      // H = J_e_x// Declare some temporariesVector2d                e, z;   // expectation, innovationMatrix2d                E, Z;   // covariances of the aboveMatrix<double, 3, 2>    K;      // Kalman gainmanif::SE2Tangentd      dx;     // optimal update step, or error-statemanif::SE2d::Jacobian   J_xi_x; // Jacobian is typedef MatrixMatrix<double, 2, 3>    J_e_xi; // Jacobian////// CONFIGURATION DONE// DEBUGcout << std::fixed   << std::setprecision(3) << std::showpos << endl;cout << "X STATE     :    X      Y    THETA" << endl;cout << "----------------------------------" << endl;cout << "X initial   : " << X_simulation.log().coeffs().transpose() << endl;cout << "----------------------------------" << endl;// END DEBUG// START TEMPORAL LOOP////// Make 10 steps. Measure up to three landmarks each time.for (int t = 0; t < 10; t++){I. Simulation ###############################################################################/// simulate noiseu_noise = u_sigmas * Array3d::Random();             // control noiseu_noisy = u_nom + u_noise;                          // noisy controlu_simu   = u_nom;u_est    = u_noisy;u_unfilt = u_noisy;/// first we move - - - - - - - - - - - - - - - - - - - - - - - - - - - -X_simulation = X_simulation + u_simu;               // overloaded X.rplus(u) = X * exp(u)/// then we measure all landmarks - - - - - - - - - - - - - - - - - - - -for (std::size_t i = 0; i < landmarks.size(); i++){b = landmarks[i];                               // lmk coordinates in world frame/// simulate noisey_noise = y_sigmas * Array2d::Random();         // measurement noisey = X_simulation.inverse().act(b);              // landmark measurement, before adding noisey = y + y_noise;                                // landmark measurement, noisymeasurements[i] = y;                            // store for the estimator just below}II. Estimation ###############################################################################/// First we move - - - - - - - - - - - - - - - - - - - - - - - - - - - -X = X.plus(u_est, J_x, J_u);                        // X * exp(u), with JacobiansP = J_x * P * J_x.transpose() + J_u * U * J_u.transpose();/// Then we correct using the measurements of each lmk - - - - - - - - -for (int i = 0; i < NUMBER_OF_LMKS_TO_MEASURE; i++){// landmarkb = landmarks[i];                               // lmk coordinates in world frame// measurementy = measurements[i];                            // lmk measurement, noisy// expectatione = X.inverse(J_xi_x).act(b, J_e_xi);           // note: e = R.tr * ( b - t ), for X = (R,t).H = J_e_xi * J_xi_x;                            // note: H = J_e_x = J_e_xi * J_xi_xE = H * P * H.transpose();// innovationz = y - e;Z = E + R;// Kalman gainK = P * H.transpose() * Z.inverse();            // K = P * H.tr * ( H * P * H.tr + R).inv// Correction stepdx = K * z;                                     // dx is in the tangent space at X// UpdateX = X + dx;                                     // overloaded X.rplus(dx) = X * exp(dx)P = P - K * Z * K.transpose();}III. Unfiltered ##############################################################################// move also an unfiltered version for comparison purposesX_unfiltered = X_unfiltered + u_unfilt;IV. Results ##############################################################################// DEBUGcout << "X simulated : " << X_simulation.log().coeffs().transpose() << endl;cout << "X estimated : " << X.log().coeffs().transpose() << endl;cout << "X unfilterd : " << X_unfiltered.log().coeffs().transpose() << endl;cout << "----------------------------------" << endl;// END DEBUG}////// END OF TEMPORAL LOOP. DONE.return 0;
}

0392760afa798dd52247e71a8424af24.png

The End

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.mzph.cn/news/129448.shtml

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

Android系统Launcher启动流程学习(二)launcher启动

Zygote&#xff08;孵化器&#xff09;进程启动 在init进程中有解析.rc文件&#xff0c;在这个rc文件中配置了一个重要的服务service–zygote&#xff0c;这是app程序的鼻祖 zygote进程主要负责创建Java虚拟机&#xff0c;加载系统资源&#xff0c;启动SystemServer进程&#…

Postgresql批量按照顺序更新某一个字段

如批量更新采购订单行sequence字段&#xff0c;按照订单行id的顺序赋值1&#xff0c;2&#xff0c;3&#xff0c;4...&#xff1a; UPDATE purchase_order_line_copy1 SET sequence subquery.new_sequence FROM (SELECT id, ROW_NUMBER() OVER (ORDER BY id) AS new_sequence…

Pytest-Allure及Allure命令使用

一、Allure介绍 Allure是Pytest用于生成测试报告的框架&#xff0c;提供丰富的测试报告功能&#xff1b; 二、Allure安装 Allure安装分为2块&#xff0c;分别是pytest-Allure库安装&#xff0c;本地生成报告并导出的命令行allure安装&#xff1b; 1、pytest-Allure库安装 …

时序预测 | Python实现ARIMA-CNN-LSTM差分自回归移动平均模型结合卷积长短期记忆神经网络时间序列预测

时序预测 | Python实现ARIMA-CNN-LSTM差分自回归移动平均模型结合卷积长短期记忆神经网络时间序列预测 目录 时序预测 | Python实现ARIMA-CNN-LSTM差分自回归移动平均模型结合卷积长短期记忆神经网络时间序列预测预测效果基本介绍程序设计参考资料 预测效果 基本介绍 时序预测 …

【vtk学习笔记4】基本数据类型

一、可视化数据的基本特点 可视化数据有以下特点&#xff1a; 离散型 计算机处理的数据是对无限、连续的空间进行采样&#xff0c;生成的有限采样点数据。在某些离散点上有精确的值&#xff0c;但点与点之间值不可知&#xff0c;只有通过插值方式获取数据具有规则或不规则的结…

机器学习 - 加油站数据分析

一、实验数据 数据集&#xff1a;“加油站数据.xls” 数据集介绍&#xff1a;该表记录了用户在11月和12月一天24小时内的加油信息&#xff0c;包括&#xff1a;持卡人标识&#xff08;cardholder&#xff09;、卡号&#xff08;cardno&#xff09;、加油站网点号&#xff08;n…

Simulink的To Workspace

To Workspace模块将Simulink产生的数据存储到matlab的工作区。 用To Workspace模块中的数据进行绘图。 参见Matlab/simulink/simscape multibody-to wotkspace模块使用_to workspace模块_五VV的博客-CSDN博客To workspace模块入门详解_哔哩哔哩_bilibili&#xff08;很好&#…

c++之类和对象

首先我们要理解cin,cout只能自动识别内置类型&#xff0c;原因就是因为cin,cout里面的函数重载。 那么如果我想输入非内置类型&#xff0c;就要进行运算符重载。 但是会发生如下的情况。 友元函数可以访问对象的私有。 运算符重载的总结 成员初始化既可以用函数体内初始化也可…

月报总结|Moonbeam 10月份大事一览

万圣节快乐&#xff01;时间一晃眼&#xff0c;10月已经迈入尾声&#xff0c;也即将迎来寒冷的冬天。但与季节相反&#xff0c;加密产业近期的发展可以说是高潮起伏&#xff0c;热度不断攀升。Moonbeam在10月中也发布了许多重大的更新&#xff0c;如Uniswap V3前段上线、众贷DO…

antd的Table组件使用rowSelection属性实现多选时遇到的bug

前言 前端样式框架采用AntDesign时&#xff0c;经常会使用到Table组件&#xff0c;如果要有实现多选或选择的需求时往往就会用到rowSelection属性&#xff0c;效果如下 rowSelection属性属性值如下 问题 文档中并没有说明选择时以数据中的哪个属性为准&#xff0c;看官方案例…

python第一课 变量

1.离线的情况下首选txt文档 2.有道云笔记 3.思维导图 xmind mindmaster 4.博客 5.wps流程图 # 变量的命名规则 1.变量名只能由数字字母下划线组成 2.变量名不能以数字开头 3.变量名不能与关键字重名 快捷键 撤销&#xff1a;Ctrl/Command Z 新建&#xff1a;Ctrl/Com…

VScode + opencv + c++ + win配置教程

准备&#xff1a; 1、下载opencv 2、下载MinGw 3、 3、下载CMake 下载完解压放到一个文件夹里面&#xff0c;便于环境管理&#xff0c;文件夹我重命名了&#xff0c;解压出来文件名不一样正常 环境变量配置 C:\Users\wuxulong\cpp_env\MinGw\mingw64\bin C:\Users\wuxulon…

linux下sqlplus登录oracle显示问号处理办法

问题描述 昨天紧急通过rpm按安装方式给客户装了一台linux的19c数据库&#xff0c;操作系统是CentOs Stream release 9&#xff0c;过程不再回忆了… 今天应用发现sqlplus登入后部分显示问号&#xff1f;&#xff0c;需要处理下 原因分析&#xff1a; 很明显&#xff0c;这就是…

2024上海国际人工智能展(CSITF)“创新驱动发展·科技引领未来”

人工智能&#xff08;Artificial Intelligence&#xff0c;AI&#xff09;作为当今世界科技发展的关键领域之一&#xff0c;正不断推动着各行各业的创新和变革。作为世界上最大的消费市场之一&#xff0c;中国正在积极努力将AI技术与产业融合并加速推广应用。在这个背景下&…

数字孪生三剑客!云渲染及虚拟仿真服务器产品MapGIS Server for Unreal来了

10月18日&#xff0c;新一代全空间智能GIS平台的升级之作——MapGIS 10.6 Pro在深圳正式发布&#xff0c;整场发布会全空间GIS、智能GIS、信创GIS、全栈开发各维度创新一一揭晓&#xff0c;全空间一体化、深度融合云端的全系产品全面升级&#xff0c;科技盛宴、高潮迭起。其中尤…

【快速解决】Android Button页面跳转功能

目录 让我们直接开始 第一步&#xff1a;先建立一个新的activity ​编辑 第二步&#xff1a;打开第一个页面的Java文件MainActivity 方法一&#xff1a;直接跳转功能如下&#xff1a; 方法二&#xff1a;输入密码才能进行跳转功能如下&#xff1a; 需要注意的地方 结语 让…

【PID专题】MATLAB如何实现PID?

MATLAB是一种非常强大的工具&#xff0c;用于实现和分析PID&#xff08;比例-积分-微分&#xff09;控制器。在MATLAB中&#xff0c;您可以使用控制系统工具箱来设计、模拟和调整PID控制系统。 以下是一般步骤&#xff0c;演示如何在MATLAB中实现PID控制&#xff1a; 1. 打开MA…

欧洲数据格式----EDF+的格式释义及与BDF的区别

EDF同EDF的区别 1&#xff1a;EDF 允许将多个非连续记录存储到一个文件中。这是与EDF唯一不兼容的地方。 2&#xff1a;EDF 标头记录可识别患者并根据 EDF 规范指定记录信号的技术特征&#xff0c;但第一个“保留&#xff08;reserved&#xff09;”字段&#xff08;44 个字符…

C/C++与圆相关的计算 2021年3月电子学会青少年软件编程(C/C++)等级考试一级真题答案解析

目录 C/C与圆相关的计算 一、题目要求 1、编程实现 2、输入输出 二、算法分析 三、程序编写 四、程序说明 五、运行结果 六、考点分析 C/C与圆相关的计算 2021年3月 C/C编程等级考试一级编程题 一、题目要求 1、编程实现 给出圆的半径&#xff0c;求圆的直径、周长…

Required String parameter ‘name‘ is not present

[org.springframework.web.bind.MissingServletRequestParameterException: Required String parameter name is not present] 服务端有参数name&#xff0c;客户端没有传上来