SAD notes

ESKF 总结

prediction

更新误差先验
在这里插入图片描述

F F F通过3.42来算
在这里插入图片描述
得到
在这里插入图片描述
这里有点绕的一点是: 误差状态的 F F F牵涉到名义状态, 而名义状态又需要在时间上推进更新

其中, F中的名义状态的推进通过公式3.41得到,

(名义状态不考虑误差, 这一点从3.41d, 3.41e可以看出, 误差状态只考虑误差, 真实状态为名义+误差)

在这里插入图片描述代码的实现

template <typename S>
bool ESKF<S>::Predict(const IMU& imu) {assert(imu.timestamp_ >= current_time_);double dt = imu.timestamp_ - current_time_;if (dt > (5 * options_.imu_dt_) || dt < 0) {// 时间间隔不对,可能是第一个IMU数据,没有历史信息LOG(INFO) << "skip this imu because dt_ = " << dt;current_time_ = imu.timestamp_;return false;}// nominal state 递推VecT new_p = p_ + v_ * dt + 0.5 * (R_ * (imu.acce_ - ba_)) * dt * dt + 0.5 * g_ * dt * dt;VecT new_v = v_ + R_ * (imu.acce_ - ba_) * dt + g_ * dt;SO3 new_R = R_ * SO3::exp((imu.gyro_ - bg_) * dt);R_ = new_R;v_ = new_v;p_ = new_p;// 其余状态维度不变// error state 递推// 计算运动过程雅可比矩阵 F,见(3.47)// F实际上是稀疏矩阵,也可以不用矩阵形式进行相乘而是写成散装形式,这里为了教学方便,使用矩阵形式Mat18T F = Mat18T::Identity();                                                 // 主对角线F.template block<3, 3>(0, 3) = Mat3T::Identity() * dt;                         // p 对 vF.template block<3, 3>(3, 6) = -R_.matrix() * SO3::hat(imu.acce_ - ba_) * dt;  // v对thetaF.template block<3, 3>(3, 12) = -R_.matrix() * dt;                             // v 对 baF.template block<3, 3>(3, 15) = Mat3T::Identity() * dt;                        // v 对 gF.template block<3, 3>(6, 6) = SO3::exp(-(imu.gyro_ - bg_) * dt).matrix();     // theta 对 thetaF.template block<3, 3>(6, 9) = -Mat3T::Identity() * dt;                        // theta 对 bg// mean and cov predictiondx_ = F * dx_;  // 这行其实没必要算,dx_在重置之后应该为零,因此这步可以跳过,但F需要参与Cov部分计算,所以保留cov_ = F * cov_.eval() * F.transpose() + Q_; current_time_ = imu.timestamp_;return true;
}

Q Q Q的算法

注意, 在离散情况下
η v = η a Δ t η θ = η g Δ t η g = η b g Δ t η a = η b a Δ t \begin{aligned} &\eta_v = \eta_a \Delta t\\ &\eta_ \theta = \eta_g \Delta t \\ &\eta_g = \eta_{bg} \Delta t\\ &\eta_ a = \eta_{ba} \Delta t \\ \end{aligned} ηv=ηaΔtηθ=ηgΔtηg=ηbgΔtηa=ηbaΔt
可以根据3.40将3.42进行一阶泰勒展开
在这里插入图片描述

代码中的实现

        double ev = options.acce_var_;double et = options.gyro_var_;double eg = options.bias_gyro_var_;double ea = options.bias_acce_var_;double ev2 = ev;  // * ev;   // tj : 为什么没平方? Q里面应该是方差double et2 = et;  // * et;double eg2 = eg;  // * eg;double ea2 = ea;  // * ea;// 设置过程噪声Q_.diagonal() << 0, 0, 0, ev2, ev2, ev2, et2, et2, et2, eg2, eg2, eg2, ea2, ea2, ea2, 0, 0, 0;

其中options的定义

    struct Options {Options() = default;/// IMU 测量与零偏参数double imu_dt_ = 0.01;  // IMU测量间隔// NOTE IMU噪声项都为离散时间,不需要再乘dt,可以由初始化器指定IMU噪声double gyro_var_ = 1e-5;       // 陀螺测量标准差double acce_var_ = 1e-2;       // 加计测量标准差double bias_gyro_var_ = 1e-6;  // 陀螺零偏游走标准差double bias_acce_var_ = 1e-4;  // 加计零偏游走标准差

correction

更新误差后验

在这里插入图片描述

H H H是观测值对误差状态变量的jacob

先看GNSS, 它观测值有位移和旋转, 根据3.66和3.70可得 H H H,
在这里插入图片描述

GNSS对旋转的观测是总体的旋转, 然而名义上的 R R R是知道的, 所以我们可以将观测值变以成对于旋转误差状态的直接观测, 这样一来, 它对自己求导就为 I I I

同理, GNSS对位移的观测是总体的位移, 然后名义上的 p p p是知道, 所以我们将对总体位移的观测转到对于误差位移的直接观测

    Eigen::Matrix<S, 6, 18> H = Eigen::Matrix<S, 6, 18>::Zero();H.template block<3, 3>(0, 0) = Mat3T::Identity();  // P部分 (3.70)H.template block<3, 3>(3, 6) = Mat3T::Identity();  // R部分(3.66)

然后求 V V V

 // 卡尔曼增益和更新过程Vec6d noise_vec;noise_vec << trans_noise, trans_noise, trans_noise, ang_noise, ang_noise, ang_noise;Mat6d V = noise_vec.asDiagonal();

其中trans_noise, ang_noise在option中定义

        /// RTK 观测参数double gnss_pos_noise_ = 0.1;                   // GNSS位置噪声double gnss_height_noise_ = 0.1;                // GNSS高度噪声double gnss_ang_noise_ = 1.0 * math::kDEG2RAD;  // GNSS旋转噪声

然后更新后验3.51b, 3.51d

    // 更新x和covVec6d innov = Vec6d::Zero();innov.template head<3>() = (pose.translation() - p_);          // 平移部分innov.template tail<3>() = (R_.inverse() * pose.so3()).log();  // 旋转部分(3.67)dx_ = K * innov;cov_ = (Mat18T::Identity() - K * H) * cov_;

有了误差状态, 就可以更新名义状态3.51c

   /// 更新名义状态变量,重置error statevoid UpdateAndReset() {p_ += dx_.template block<3, 1>(0, 0);v_ += dx_.template block<3, 1>(3, 0);R_ = R_ * SO3::exp(dx_.template block<3, 1>(6, 0));if (options_.update_bias_gyro_) {bg_ += dx_.template block<3, 1>(9, 0);}if (options_.update_bias_acce_) {ba_ += dx_.template block<3, 1>(12, 0);}g_ += dx_.template block<3, 1>(15, 0);ProjectCov();dx_.setZero();}

注意 这里有一步需要投影, 参考3.63

在这里插入图片描述

/// 对P阵进行投影,参考式(3.63)
void ProjectCov() {Mat18T J = Mat18T::Identity();J.template block<3, 3>(6, 6) = Mat3T::Identity() - 0.5 * SO3::hat(dx_.template block<3, 1>(6, 0));cov_ = J * cov_ * J.transpose();
}

再看odom, 它观测值只有速度

首先求H, 注意这里观测值为本地速度, 但是名义变量 R R R这时是知道的, 所以可以把观测值从本地速度转化到世界速度, 然后世界速度对世界速度求导就为 I I I

在这里插入图片描述

// odom 修正以及雅可比
// 使用三维的轮速观测,H为3x18,大部分为零
Eigen::Matrix<S, 3, 18> H = Eigen::Matrix<S, 3, 18>::Zero();
H.template block<3, 3>(0, 3) = Mat3T::Identity();

然后算V

// 设置里程计噪声
double o2 = options_.odom_var_ * options_.odom_var_;
odom_noise_.diagonal() << o2, o2, o2;

option里面的定义

/// 里程计参数
double odom_var_ = 0.5;
double odom_span_ = 0.1;        // 里程计测量间隔
double wheel_radius_ = 0.155;   // 轮子半径
double circle_pulse_ = 1024.0;  // 编码器每圈脉冲数

然后更新后验

本地速度观测值的算法参见 3.76

// velocity obs
double velo_l = options_.wheel_radius_ * odom.left_pulse_ / options_.circle_pulse_ * 2 * M_PI / options_.odom_span_;
double velo_r =options_.wheel_radius_ * odom.right_pulse_ / options_.circle_pulse_ * 2 * M_PI / options_.odom_span_;
double average_vel = 0.5 * (velo_l + velo_r);VecT vel_odom(average_vel, 0.0, 0.0);
VecT vel_world = R_ * vel_odom;dx_ = K * (vel_world - v_);// update cov
cov_ = (Mat18T::Identity() - K * H) * cov_;
UpdateAndReset();

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

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

相关文章

React-Redux总结含购物车案例

React-Redux总结含购物车案例 reduc简介 redux是react全家桶的一员&#xff0c;它为react给i共可预测化的状态管理机制。redux是将整个应用状态存储到一个地方&#xff0c;成为store,里面存放着一颗树状态(state,tree),组件可以派发dispatch行为action给store,而不是直接通知其…

Spring Authorization Server 1.1 扩展 OAuth2 密码模式与 Spring Cloud Gateway 整合实战

目录 前言无图无真相创建数据库授权服务器maven 依赖application.yml授权服务器配置AuthorizationServierConfigDefaultSecutiryConfig 密码模式扩展PasswordAuthenticationTokenPasswordAuthenticationConverterPasswordAuthenticationProvider JWT 自定义字段自定义认证响应认…

SpringCloud 微服务全栈体系(四)

第六章 Nacos 配置管理 Nacos 除了可以做注册中心&#xff0c;同样可以做配置管理来使用。 一、统一配置管理 当微服务部署的实例越来越多&#xff0c;达到数十、数百时&#xff0c;逐个修改微服务配置就会让人抓狂&#xff0c;而且很容易出错。我们需要一种统一配置管理方案…

ubuntu执行普通用户或root用户执行apt-get update时报错Couldn‘t create temporary file /tmp/...

apt-get update无法更新&#xff0c;报错&#xff1a; Couldnt create temporary file /tmp/apt.conf.GSzv74 for passing config to&#xff0c;&#xff0c;&#xff0c; 这是由于/tmp目录没有权限导致的&#xff0c;解决办法&#xff1a; chmod 777 /tmp

Redis 配置文件(redis.conf)中文注释及说明

文章目录 一、概述二、觉见基础配置1.1 导入另一个配置文件1.2 添加Redis扩展1.3 绑定Redis服务在那些网卡上&#xff0c;也就是远程可以通过那个的IP地址访问。1.2 指定Redis服务监听端口1.2 最大分配内容大小1.2 后台服务方式运行1.2 日志记录文件1.2 添加扩展 三、完整配置文…

Photoshop(PS)安装教程(图文教程超详细)

目录 一.简介 二.安装步骤 软件&#xff1a;PS版本&#xff1a;2023语言&#xff1a;简体中文大小&#xff1a;3.20G系统要求&#xff1a;Win10&#xff08;1903&#xff09;及以上版本&#xff0c;64位操作系统硬件要求&#xff1a;CPU2.0GHz 内存8G(或更高&#xff0c;不支…

Open3D(C++) 最小二乘拟合平面(直接求解法)

目录 一、算法原理二、代码实现三、结果展示本文由CSDN点云侠原创,原文链接。 一、算法原理 平面方程的一般表达式为: A x + B y + C

力扣每日一题74:搜索二维矩阵

给你一个满足下述两条属性的 m x n 整数矩阵&#xff1a; 每行中的整数从左到右按非严格递增顺序排列。每行的第一个整数大于前一行的最后一个整数。 给你一个整数 target &#xff0c;如果 target 在矩阵中&#xff0c;返回 true &#xff1b;否则&#xff0c;返回 false 。…

Zabbix安装与部署

前言 Zabbix是一个开源的网络监控和系统监控解决方案&#xff0c;用于监控服务器、网络设备、应用程序和服务。它基于客户端-服务器体系结构&#xff0c;使用多种监控选项来监控不同类型的设备和应用程序。Zabbix支持数据收集、处理和存储&#xff0c;以及报警和可视化等功能。…

科技资讯|苹果穿戴新专利,表带、服装等织物可变身柔性屏幕或扬声器

根据美国商标和专利局&#xff08;USPTO&#xff09;本周公示的清单&#xff0c;苹果公司获得了一项新的技术专利&#xff0c;可以在 Apple Watch 表带、服装等物品上&#xff0c;引入基于织物的柔性扬声器。 根据专利描述&#xff0c;通过在织物中嵌入声学组件&#xff08;例…

【Linux】CentOS8.4 安装docker

&#x1f984; &#x1f390;个人主页 &#x1f390;✨&#x1f341; &#x1fa81;&#x1f341;&#x1fa81;&#x1f341;&#x1fa81;&#x1f341; 感谢点赞和关注 &#xff0c;每天进步一点点&#xff01;加油&#xff01;&#x1fa81;&#x1f341;&#x1fa81;&…

达芬奇MacOS最新中文版 DaVinci Resolve Studio 18中文注册秘钥

DaVinci Resolve Studio 18是一款专业的视频编辑软件&#xff0c;它具有多种强大的功能。首先&#xff0c;它提供了丰富的视频剪辑工具&#xff0c;如剪切、复制、粘贴、剪辑、缩放和移动等&#xff0c;使用户可以轻松地剪辑和组合视频素材。其次&#xff0c;该软件还支持多个轨…

Python第三方库 - Flask(python web框架)

1 Flask 1.1 认识Flask Web Application Framework&#xff08; Web 应用程序框架&#xff09;或简单的 Web Framework&#xff08; Web 框架&#xff09;表示一个库和模块的集合&#xff0c;使 Web 应用程序开发人员能够编写应用程序&#xff0c;而不必担心协议&#xff0c;线…

定档11月2日,YashanDB 2023年度发布会即将启航

数据库作为支撑核心业务的关键技术&#xff0c;对数字经济的发展有着重要的支撑作用&#xff0c;随着云计算、AI等技术的迅猛发展和数据量的爆发式增长&#xff0c;推动着数据库技术的加速创新。 为了应对用户日益复杂的数据管理需求&#xff0c;赋能行业国产化建设和数字化转型…

unity 一键替换 UI上所有字体,批量替换字体(包括:Text和Text (TMP))

前言&#xff1a;在开发中会遇到这种情况&#xff0c;开发完了&#xff0c;发现UI字体没有替换&#xff0c;特别是需要发布到WebGL端的同学&#xff0c;突然发现无法显示汉字了。下面一个非常方便的方法完美解决。 1.解压出来的脚本放在Edit文件下&#xff0c;没有的创建一个 2…

2016年亚太杯APMCM数学建模大赛A题基于光学信息数据的温度及关键元素含量预测求解全过程文档及程序

2016年亚太杯APMCM数学建模大赛 A题 基于光学信息数据的温度及关键元素含量预测 原题再现 光含有能量&#xff0c;在一定条件下可以转化为热。燃烧是一种常见的现象&#xff0c;既能发光又能发热。光和热通常是同时存在的&#xff0c;一般来说&#xff0c;光强度越高&#xf…

2023年香水行业数据分析:国人用香需求升级,高端香水高速增长

在人口结构变迁的背景下&#xff0c;“Z世代”作为当下我国的消费主力&#xff0c;正在将“悦己”消费推动成为新潮流。具备经济基础的“Z世代”倡导“高颜值”、“个性化”、“精致主义”&#xff0c;这和香水、香氛为代表的“嗅觉经济”的特性充分契合&#xff0c;因此&#…

【计算机网络笔记】网络应用对传输服务的需求

系列文章目录 什么是计算机网络&#xff1f; 什么是网络协议&#xff1f; 计算机网络的结构 数据交换之电路交换 数据交换之报文交换和分组交换 分组交换 vs 电路交换 计算机网络性能&#xff08;1&#xff09;——速率、带宽、延迟 计算机网络性能&#xff08;2&#xff09;…

USB学习(2):USB端点和传输协议(数据包、事物)详解

接着上一篇文章USB学习(1)&#xff1a;USB基础之接口类型、协议标准、引脚分布、架构、时序和数据格式&#xff0c;继续介绍一下USB的相关知识。 文章目录 1 USB端点(Endpoints)1.1 基本知识1.2 四种端点 2 传输协议2.1 数据包类型2.1.1 令牌数据包(Token packets)2.1.2 数据数…

【机器学习合集】激活函数合集 ->(个人学习记录笔记)

文章目录 综述1. S激活函数(sigmoid&Tanh)2. ReLU激活函数3. ReLU激活函数的改进4. 近似ReLU激活函数5. Maxout激活函数6. 自动搜索的激活函数Swish 综述 这些都是神经网络中常用的激活函数&#xff0c;它们在非线性变换方面有不同的特点。以下是这些激活函数的主要区别&am…