程序员经验分享-hwhale

寻找APM中EKF的五大公式

2023-05-16

EKF核心代码位置

AP_NavEKF2.cpp
在这里插入图片描述
在这里插入图片描述
进入该函数
## 标题
进入该函数，然后可以看到关键部分，也即卡尔曼五个公式的地方。
在这里插入图片描述
下面介绍每个公式的具体位置

28状态值

首先要知道选用的状态值有哪些，28状态值（但参与EKF方差估计的只有24个，不计算四元数的协方差，FIX4中的EKF是24状态值，没有0~2）
在这里插入图片描述

公式一：x（k+1|k）=F*x(k|k)+Q

四元数更新↓
在这里插入图片描述
速度位置更新↓
在这里插入图片描述
调用的相关变量如下↓
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述

总结

IMU的数据用于预测过程，
IMU得到的角度增量用于更新四元数
IMU得到的速度增量用于更新速度和位置
更新姿态角涉及到状态值【9-14，上一时刻的26~28】
更新速度涉及到旋转矩阵及状态值【15】，也即涉及到状态值【9-15，上一时刻的26~28】
更新位置涉及到速度，也即涉及到状态值【3-5，9-15，上一时刻的26~28】
由此发现状态值的更新是非线性的，需要通过求偏导得到状态转移矩阵，和噪声驱动矩阵，从而进行P的第一次更新。

公式二：P(k+1|k) = FP(k|k)Ft + GQGt

进入下面该函数↓
在这里插入图片描述
得到过程噪声 ，注意没有四元数，共24维。其中进行了状态更新的速度位置和误差角，即状态量的【0~8】初始化为0
在这里插入图片描述
其他几个值分别初始化为↓
在这里插入图片描述
nextP即为更新后的P值，这里为最后的输出值，代码中完整公式为↓
在这里插入图片描述
nextP的来源就是根据上述公式，结果较长，这里不粘贴，知道是通过雅克比求偏导得来的就行。
NextP求完，复制给P
在这里插入图片描述

公式三：K=P（k+1|k） * Ht * 1/S(k)

下面只以磁力计为例
首先得到创新数据，也即H*stateStruct — MagMea,如下↓
在这里插入图片描述
其中MagPred，来源于状态值stateStruct.quat、stateStruct.earth_magfield、stateStruct.body_magfield↓
在这里插入图片描述
初始化测量噪声↓
在这里插入图片描述
根据公式得到K↓
在这里插入图片描述

公式四：x(k+1|k+1) = x(k+1|k) + K * (innovX)

在这里插入图片描述

公式五：P(k+1|k+1) =(I - K*H) P(k+1|k)

在这里插入图片描述
自此，卡尔曼的五个公式完成了，也得到了新的stateStruct值。
然而，最终采用的估计值并非是EKF的输出值，而是有用新的IMU的与EKF的输出值进行融合，作为最终的估计值，存放在outputState中。
APM的EKF花了大量的代码来限制噪声协方差的大小，以及根据计算的方差来判断是否进行融合等，更精华的地方或许也在此。

飞控学习告一段落，接下来学习slam了。

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)

APM

EKF

五大公式

寻找APM中EKF的五大公式 的相关文章

  • PX4 APM ROS MAVROS Gazebo之间关系

    https www cnblogs com yilangUAV p 14476923 html 1 PX4与APM 参考 https bbs amovlab com forum php mod 61 viewthread amp tid 6

  • px4 EKF中Q、R阵设置的思考

    关于Q R的讨论 Q阵 xff0c 状态转移误差矩阵 xff0c 代表从Xt 1到Xt过程中 xff0c 状态转移和真实过程之间的误差 xff0c 具体其中变量可能是对状态转移有影响的变量 xff0c 比如在有一些场景下 xff0c 可能为

  • APM和PIX飞控日志分析入门贴

    我们在飞行中 xff0c 经常会碰到各种各样的问题 xff0c 经常有模友很纳闷 xff0c 为什么我的飞机会这样那样的问题 xff0c 为什么我的飞机会炸机 xff0c 各种问题得不到答案是一件非常不爽的问题 xff0c 在APM和PIX

  • Mission Planner中级应用（APM或PIX飞控）1——振动测量

    https ardupilot org planner docs common measuring vibration html highlight 61 vibrat 以上为参考网站 测量振动 自动驾驶仪具有对振动敏感的加速度计 这些加速

  • Mission Planner中级应用（APM或PIX飞控）2——飞控减振

    飞控减振 自动驾驶仪具有对振动敏感的加速度计 这些加速度计值与气压计和GPS数据来估计无人机的位置 随着过度振动 xff0c 依赖精确定位的模式下的性能 xff08 例如在无人机上 xff1a AltHold Loiter RTL Guid

  • APM代码调试知识点汇总

    由于项目的需要 xff0c 对ardupilot的源码进行二次开发 本文记录在二次开发中遇到的问题以及注意事项 xff1a CUAV V5 实测 apm 串口 xff0c 对于姿态数据的发送和接收在200Hz的时候 xff0c 是没有问题的

  • APM日志格式表

    APM的日志格式文件在libraries AP Logger LogStructure h文件中 xff0c 二进制数据类型对照表如下 xff1a 单位对照表如下 xff1a

  • PX4_ECL_EKF代码分析1

    写在前面 源码版本 xff1a 1 6 0rc1 源码位置1 xff1a Firmware 1 6 0rc1 src modules ekf2 main cpp 源码位置2 xff1a Firmware 1 6 0rc1 src lib e

  • PX4_ECL_EKF代码分析2

    写在前面 源码版本 xff1a 1 6 0rc1 源码位置1 xff1a Firmware 1 6 0rc1 src modules ekf2 main cpp 源码位置2 xff1a Firmware 1 6 0rc1 src lib e

  • 从程序中学习EKF-SLAM（一）

    在一次课程的结课作业上 xff0c 作业要求复写一个EKF SLAM系统 xff0c 我从中学到了好多知识 作为一个典型轻量级slam系统 xff0c 这个小项目应该特别适合于slam系统入门 xff0c 可以了解到经典卡尔曼滤波器在sla

  • 控制算法学习 四、扩展卡尔曼滤波EKF

    控制算法学习 四 扩展卡尔曼滤波EKF 前言非线性系统状态 观测方程线性化扩展卡尔曼滤波EKF后记 前言 经典卡尔曼滤波的使用场景是线性系统 xff0c 但现实应用时 xff0c 大多数系统都是非线性的 扩展卡尔曼滤波 xff08 Exte

  • APM-MAVROS连接

    1 运行mavros roslaunch mavros apm launch fcu url 61 34 dev ttyUSB0 921600 34 2 读取topic之前先运行以下命令 xff0c 修改飞控广播频率 rosservice

  • APM、PIXHAWK、PX4的关系

    摘自 xff1a https bbs amovlab com forum php mod 61 viewthread amp tid 61 1202 amp extra 61 page 3D1 Pixhawk PX4 APM傻傻分不清 xf

  • APM添加超声模块及定高程序分析

    给飞控添加新的模块 xff0c 通常的做法是写驱动文件 xff0c 然后用uORB订阅消息 xff0c 这种方法已经有文章介绍了 xff0c 下面介绍另一种更加简洁的方法 硬件连接 UARTD xff08 ttyS2 xff09 超声 Bo

  • 关于apm飞控烧bootloader

    最近一直由于学校大创申请的的四轴 xff0c 一直在做这玩意 xff0c 哎 xff0c 无奈这货实在不像想象的那样简单 xff0c 自己写了N久飞控 xff0c 也没把飞机飞起来 xff0c 只能先用开源的apm飞控练练手呗 xff0c

  • APM（pixhawk）飞控疑难杂症解决方法汇总（持续更新）

    原文链接 xff1a http www nufeichuiyun com p 61 28

  • APM 学习 6 --- ArduPilot 线程

    ArduPilot 学习之路 6 xff0c 线程 英文原文地址 xff1a https ardupilot org dev docs learning ardupilot threading html 理解 ArduPilot 线程 线程

  • APM添加参数

    APM添加参数 参考 https ardupilot org dev docs code overview adding a new parameter html 添加应用参数模块 例如 battery Parameters h k par

  • Java探针-Java Agent技术-阿里面试题 javaagent 动态字节码修改 skywalking -无侵入探针深入理解

    关注UAV MOF工作原理 同创和dynatrace探针如何重启preload注入的 Docker 动态修改容器中的环境变量 动态修改 java JAVA OPTS linux进程启动拦截判断 0 好像说明白了一些 Java Agent 一

  • zipkin接入mysql【windows】

    java jar zipkin jar 这种方式启动数据是保存在内存中的 下面我们配置一下将数据保存到mysql中 创建数据库 CREATE DATABASE zipkin 创建表结构 表结构内容参考以下连接 https github co

随机推荐

  • ERROR: Could not install packages due to an OSError: [Errno 13] Permission denied问题解决

    Windows Anaconda python3 6 安装依赖包发生错误如下 pip install i https pypi tuna tsinghua edu cn simple r requirements txt user ERRO

  • docker的深入浅出--3.Dockerfile介绍及保留字指令的使用run、entrypoint、onbuild、add和copy关键字以及自定义镜像

    目录 一 Dockerfile介绍 1 centos镜像来理解Dockerfile 2 docker的创建流程 二 Dockerfile的保留字指令 1 自定义centos镜像 xff08 run保留字 xff09 history指令 2

  • 通过MAVROS控制仿真无人机

    首先 xff0c 在目录中建立工作区 xff0c 并进行初始化操作 mkdir p catkin ws src cd catkin ws catkin init wstool init src rosinstall generator ro

  • Jetson nano+T265+PX4实现室内定点飞行

    目录 前言 一 MAVROS的安装 二 Realsense SDK和Realsense ROS的安装 四 给予串口权限 五 在QGC中修改PX4参数 六 启动VIO节点 七 参考文献 前言 1 硬件 飞控 xff1a Pixhawk 6C

  • ros通信之topic通信机制及基于topic的节点通信

    现在的我对于节点node和节点句柄nodehandle的粗俗的理解是 xff1a 节点 xff0c 就是一个进程 xff0c 在ros的环境中叫做节点node 在计算机的工作中叫做进程 xff0c 两者是同时的 xff0c 在ros中 xf

  • inter realsener D435 ROS驱动安装（非源码编译）

    1 安装公钥 sudo apt key adv keyserver keys gnupg net recv key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE sudo apt key adv keys

  • 斜率与倾斜角的关系

    k 61 tan 61 atan k k 斜率 倾斜角

  • NVIDIA JETSON TX2 安装扩展SATA硬盘

    安装扩展硬盘 在dash中搜素disk 进入磁盘管理工具 xff0c 可以看到我们的扩展硬盘 点击磁盘左下角设置按钮 xff0c 进入Format Partition 为硬盘起个名字 xff0c 比如JetsonSSD 250 xff0c

  • Qt——QMessageBox类详解

    QMessageBox类提供了一个模式对话框 xff0c 用于通知用户或询问用户问题并接收答案 我们先来看下QMessageBox information函数的使用 其原型 xff1a static int QMessageBox info

  • 【UWB定位】 - DWM1000模块调试简单心得 - 3

    UWB定位 DWM1000模块调试简单心得 1 UWB定位 DWM1000模块调试简单心得 2 前俩篇介绍了简单的一基站一标签TOF方式测距 xff0c 第三篇我们来搭建一个 一标签三基站 的定位demo 目的 标签与三个基站分别测距 xf

  • 51单片机——计数器与定时器的区别

    定时器和计数器是同一器件 计数器 其共同的特点是都有一个计数脉冲输入端 每输入一个脉冲 计数器就进行加1或减1计数 若计数器件的计数脉冲的频率固定 则可利用计数实现定时 这就是定时器 若计数器件的作用仅仅是记录输入脉冲的多少 则称为计数器

  • vue 中 如何修改【数组中】【对象的值】,解决步骤如下

    原创 https segmentfault com q 1010000012375354 a 1020000012377603 vue 中 如何修改 数组中 对象的值 通过数组的变异方法 xff08 Vue数组变异方法 xff09 我们可以

  • 英伟达Jetson TX2 资源贴

    NVIDIA JETSON TX2 install packages 原创博客 xff0c 欢迎转载 xff0c 请注明博客链接 xff1a 英伟达Jetson TX2 资源贴 资源汇总 jetson tx2 GPIO 解决方案汇总 Jet

  • 研究线程锁之RLock（一）

    死锁 xff1a 是指两个或两个以上的进程或线程在执行过程中 xff0c 因争夺资源而造成的一种互相等待的现象 xff0c 若无外力作用 xff0c 它们都将无法推进下去 此时称系统处于死锁状态或系统产生了死锁 xff0c 这些永远在互相等

  • 虚拟机中使用OpenGL遇到的错误总结

    由于VMware对OpenGL的支持有限 xff0c 目前最新版本的VMware workstation15 Pro只支持到OpenGL3 3的core profile xff08 核心模式 xff09 xff0c 在有条件的前提下建议安装

  • 视觉SLAM——视觉里程计解决方案分析（间接法）

    目录 基本问题 分析各类求解方案优缺点分析 基本问题 视觉里程计是视觉SLAM技术的起点 xff0c 其核心问题同SLAM技术一样 xff0c 主要是定位与构图 xff0c 但视觉里程计解决的核心是定位问题 xff0c 也就是相机的位姿 通

  • 视觉SLAM理论——位姿的理解与间接求解

    目录 xff1a 位姿的定义位姿与变换矩阵的区别与联系位姿的求解方法 位姿的定义 在SLAM中 xff0c 位姿是世界坐标系到相机坐标系的变换 xff0c 包括旋转与平移 根据以上定义可以衍生以下几个问题 xff1a 1 世界坐标系在哪 x

  • 线性最小均方误差算法（LMSE），最小二乘法（LS）

    目录 背景正交投影引理LMSE算法LS算法直线拟合 背景 对于一个系统 xff0c 在给予一定的输入 xff0c 那么通常都会产生相对应的输出 在实际的系统中 xff0c 这样的输出必然伴随着噪声 xff0c 这样被噪声污染的输出通常是传感

  • 无人机，动力系统建模

    建模目的 无人机动力系统包括 xff1a 螺旋桨 电机 电调及电池 建模流程图如下 xff08 图片来源 多旋翼飞行器设计与控制 M 全权 xff09 xff1a 经过误差结算后 xff0c 将误差信息转换为螺旋桨的升力与转矩 xff0c

  • 寻找APM中EKF的五大公式

    EKF核心代码位置 AP NavEKF2 cpp 进入该函数 进入该函数 xff0c 然后可以看到关键部分 xff0c 也即卡尔曼五个公式的地方 下面介绍每个公式的具体位置 28状态值 首先要知道选用的状态值有哪些 xff0c 28状态值

热门标签