国外网站的设计风格jsp网站开发期末大作业

国外网站的设计风格,jsp网站开发期末大作业,wordpress适合seo,平台网站建设开票开什么内容误差状态卡尔曼滤波(error-state Kalman Filter)#xff0c;扩展卡尔曼滤波#xff0c;实现GPSIMU融合#xff0c;EKF ESKF GPSIMU 发现了一个讲卡尔曼滤波特别好的视频#xff0c;但是需要科学上网。卡尔曼滤波视频 最近在学习卡尔曼滤波器#xff0c;今天抽出点儿时间…误差状态卡尔曼滤波(error-state Kalman Filter)扩展卡尔曼滤波实现GPSIMU融合EKF ESKF GPSIMU发现了一个讲卡尔曼滤波特别好的视频但是需要科学上网。卡尔曼滤波视频最近在学习卡尔曼滤波器今天抽出点儿时间总结一下在这篇博客中我将会向你解释GPS融合IMU的扩展卡尔曼的推导过程并且还会提供完整的源代码和数据。首先来看一张实验结果下图是我通过仿真gps和imu数据得到的融合结果GPS的误差大约在5米IMU的是中等精度可以看到通过卡尔曼融合之后误差降低非常多。红色的GPS的测量数据蓝色是轨迹的真值绿色是融合之后的轨迹可以看到融合了imu之后的轨迹相比于只有GPS的情况提升了很多甚至与真实值都非常接近了。1.前言卡尔曼滤波器在1958年被卡尔曼等人提出之后经历了60多年这期间有各种变体被提出来但是核心的思想并没有变化。这其中比较突出的工作有EKF、IEKF、ESKF、UKF等等。它们的方程有一些差异但是用法基本区别不大只要学会应用其中的一种别的就问题不大了。卡尔曼滤波器的公式推导并不重要!卡尔曼滤波器的公式推导并不重要!卡尔曼滤波器的公式推导并不重要!如果你初学卡尔曼滤波器千万不要在公式推导和理解上花费太多的精力这将会得不偿失根据我的学习经验想直接从卡尔曼的公式推到上去理解用法是比较难的但是先不管原因边应用边学习将会是一个非常正确的做法对于理解卡尔曼滤波器将会事半功倍2.状态误差卡尔曼Error-State Kalman FilterESKF本篇博客我们将探索一下状态误差卡尔曼(ESKF)的应用它是卡尔曼滤波器的变种中应用最为广泛的一种与EKF一样它也是一种针对时变系统的非线性滤波器。但是与EKF不同的是它的线性化是总是在0附近因此线性化更准确。根据我的经验绝大部分的场景ESKF就足够使用了。如果对于滤波有更高的要求可以选择UKF甚至PF(例子滤波)。2.1 状态方程的推导在融合IMU和GPS的数据时因为IMU的频率更高所以常常用IMU的姿态解算作为轨迹增量的预测如果使用EKF滤波器那么就是这种做法。由于我们这里介绍的是更为复杂的ESKF所以这里并不是对导航信息做滤波而是对导航信息中的误差进行滤波因为误差是小量线性化时更精确。这里直接给出IMU的误差方程由于误差方程的推导比较复杂需要的知识比较多而且这里主要强调的是ESKF的用法所以就忽略IMU误差方程的推导。如果你想系统学习IMU误差方程的推导推荐你看《捷联惯导算法与组合导航原理》严恭敏你也可以直接记住下面的结论先继续往后学习当你把整个流程都走一遍之后你就明白了IMU误差方程其中然后将以上变量全部带入公式0得到如下式子这里稍微解释一下误差方程由于在做IMU的解算时做了一些近似而那些舍掉的东西就给IMU解算带来了误差。ESKF要做的就是对误差进行滤波获得当前情况下误差的最优估计然后将计算出来的位移、速度、姿态等量减去这个误差修正由于近似带来的不准确。IMU误差方程的推导可以参考《捷联惯导算法与组合导航原理》(严恭敏)在滤波器中状态方程一般都是写为如下形式其中就是我们要估计的状态量对于IMUGPS的融合系统需要估计的状态量可以有多种选择这里我们选位移误差、速度误差、姿态误差、陀螺仪的bias误差、加速度计的bias误差将其写成向量如下其中根据123等式将状态量X XX带入方程4则可以得到一个完整的其中是当前t时刻IMU body frame到导航系n的变换对于4式已经给出了的完整形式对于和它们分别代表了噪声转移矩阵和IMU中陀螺仪与加速度计的bias噪声它们的完整形式如下其中前三个是陀螺仪在三个轴上的bias噪声后三个是加速度计在三个轴上的bias噪声。而的完整形式如下2.2 观测方程的推导前面说的预测值是来自于IMU而对于观测值我们采用的是GPS的观测。在滤波器中观测方程的形式比较统一一般写为在IMU和GPS的融合中GPS观测量一般之后位置所以Y YY的完整形式如下由5式状态量的形式可以推算出和分别如下等式7中的是观测噪声它是由于GPS传感器不准确带来的它的完整形式如下等式4和7就是IMUGPS的系统下的状态方程和测量方程。2.3 构建ESKF滤波器首先我们先写出ESKF滤波器的五个经典公式ESKF和EKF本身没有区别只不过ESKF是使用的EKF滤波器对误差进行滤波这五个公式就是ESKF滤波的基础。根据2.1节和2.2节的推导我们已经获得了IMUGPS系统的状态方程和测量方程现在我们要做的就是将状态方程和测量方程应用到卡尔曼滤波器的五个公式中。对于状态方程和测量方程的推导都是在连续时间下完成的想要应用到滤波器中必须要离散化离散化时按照采样时间进行离散化。对于4式中的采用一阶泰勒近似可以得到如下形式公式9的实现过程点此进入而的离散化形式为其中T为卡尔曼的滤波周期公式10的实现过程点此进入对于观测方程本生就是离散化的所以可以直接使用。将式子910带入到8式中得到最终的卡尔曼滤波器方程为等式组(11)中的前两个等式对应的是预测过程它主要用来预测状态量和状态量对应的协方差第三个等式的是卡尔曼增益它是用来决策当前次的预测和测量中更应该相信谁。第四个和第五个等式则是矫正前两个方程预测时的误差。公式组11的五个公式的代码实现过程分别是①、②、③、④、⑤至此整个基于IMU和GPS的状态误差卡尔曼滤波ESKF推导完成。ESKF是对状态的误差进行矫正所以ESKF每次迭代完成之后必须要将状态量X XX重新设置为零但是协方差矩阵P PP中的数值需要保留代码在此。对于公式11中的它对应的就是测量误差对于GPS和IMU的系统它的计算方法是GPS的测量值减去IMU的预测值代码在此每次使用卡尔曼迭代完成之后都需要将最终得到的误差重新带回到预测的位置、速度、方向中消除掉这个误差,代码在此上面只是把结论给出来了如果确实需要了解其中卡尔曼滤波器的推导过程可以参考《机器人学中的状态估计》或者《Quaternion kinematics for the error-state Kalman filter》2.4 可观测性和可观测度分析可观测性分析在设计滤波器时有些状态量是不可观如果此时继续将这些状态量带入滤波器进行滤波可能导致滤波器不收敛或者收敛到一个错误的值。可观测度就是用来判断状态量是否都会收敛。可观测度分析可观测分析是为了评估状态量在滤波时收敛的快慢和收敛的精度对于一些可观测度非常小的状态量可以考虑进行限制或者直接扔掉。本文所讲的IMUGPS滤波系统是一种时变系统对于时变系统它的可观测性和可观测度分析可以参考论文《Observability Analysis of Piece-Wise Constant Systems》这里我不再展示详细的推导过程直接给出结论如果你对这部分内容有兴趣请阅读上面这篇论文并且在我的代码中也给出了实现过程点此进入你可以参考。IMUGPS的扩展卡尔曼滤波器系统可观测度和可观测性分析结论载体静止或着匀速运动时航向角 x 轴加速度bias和 y 轴加速度bias均不可观而且z 轴角速度bias虽然收敛但是收敛较慢载体加减速时 所有状态量基本都可观z轴角速度零偏x 轴加速度biasy轴加速度bias收敛较慢需要几十秒但是比静止和匀速运动时提升了很多载体转向时 所有状态量基本都可观但是航向角和加速度计bias以及角速度计bias参数收敛速度有一些慢需要几十秒才能收敛但是比静止和匀速运动时提升了很多。2.5 总结上面展示的这些流程就是从IMU的误差方程到扩展卡尔曼滤波器的所有流程其中很多的推导比较难理解我能省略的就尽量省略了。但是有以下的一些点你需要注意上面的IMU误差方程是基于高精度的IMU推导的考虑了地球的转速了如果你的IMU精度一般你可以忽略地球的转速也就是上式6中的 设置为0矩阵并不会对精度产生很大的影响另外有一点你一定要格外的注意上面方程的推导都是基于导航坐标系的而你的IMU结果是针对body坐标系的切记不要弄混了坐标系矩阵是加速度计敏感到的加速度在导航系下的表示代码中不要忘记了这一步另外还要注意这里的加速度不需要去掉重力加速度还要格外的注意单位的统一GPS的测量值是经纬高要转换到导航系下表示成x , y , z x,y,zx,y,z单位是米并且IMU的角速度是以弧度为基础的卡尔曼滤波器公式11中的第一个等式是状态的传递方程后面有一个噪声项你可以加上也可以忽略根据你的实际情况去决定是否需要。我的实验结果是加不加影响不大。3.代码实现在代码的文件夹/data/raw_data中我提供了方针的IMU和GPS数据它们的生成是通过gnss-ins-sim关于gnss-ins-sim的使用方法作者提供了比较详细的使用教程你按照教程就能学会它的使用方法另外你需要注意的是不要把坐标系弄混了。我在eskf-gps-imu-fusion文件夹下的gnss-ins-sim文件中放了我所使用的脚本和运动控制文件你按照readme中的方法操作就能实现数据仿真了。融合过程中产生的轨迹和融合数据都保存在data文件夹下它们都是以kitti的姿态格式保存的如果你想可视化它们我推荐你使用evo我写过一篇介绍它的使用方法的博客点击入口如果你不会使用可以参考一下。安装好evo之后使用如下命令可以显示运行之后的轨迹evo_traj kitti fused.txt gt.txt measured.txt -p对于滤波器误差噪声的调节是在config文件夹下的config.yaml文件中。当你运行起我的代码之后你可以或者如下的这样融合轨迹一定要把上面的公式和代码结合起来看如果实在难以理解可以先把这个流程和实现过程弄明白然后再边用变理解不然你可能很难学会