Fast-LO2基础知识

关于协方差P： **Q：** 我找不到P(协方差)矩阵实际更新的部分，以表示预测的不确定性。在"预测"步骤中，P的估计值是从它之前的值A和Q计算出来的。据我所知，A(系统矩阵)和Q(噪声协方差)是恒定的。然后在"校正"期间，根据K、H和预测的P计算P。H(观测矩阵)是恒定的，因此影响P的唯一变量是K(卡尔曼增益)。但是K是从预测的P，H和R(观测噪声)计算出来的，它们要么是常数，要么是P本身。那么，方程中使P与x相关的部分在哪里？对我来说，似乎P在这里递归循环，只依赖于P的常量和初始值，这没有任何意义。我遗漏了什么？ **A：** You are not missing anything. It can come as a surprise to realise that, indeed, the state error covariance matrix (P) in a linear kalman filter does not depend on the the data (z). One way to lessen the surprise is to note what the covariance is saying: it is how uncertain you should be in the estimated state, given that the models you are using (effectively A,Q and H,R) are accurate. It is not saying: this is the uncertainty. By judicious tweaking of Q and R you could change P arbitrarily. In particular you should not interpret P as a 'quality' figure, but rather look at the observation residuals. You could, for example, make P smaller by reducing R. However then the residuals would be larger compared with their computed sds. When the observations come in at a constant rate, and always the same set of observations, P will tend to a steady state that could, in principal, be computed ahead of time. However there is no difficulty in applying the kalman filter when you have varying times between observations and varying sets of observations at each time, for example if you have various sensor systems with different sampling periods. In this case you will see more variation in P, though again in principal this could be computed ahead of time. Further the kalman filter can be extended (in various ways, eg the extended kalman filter and the unscented kalman filter) to handle non linear dynamics and non linear observations. In this case because the transition matrix (A) and the observation model matrix (H) have a state dependency, so too will P. 你没有错过任何东西。 令人惊讶的是，事实上，线性卡尔曼滤波器中的状态误差协方差矩阵(P)并不依赖于数据(z)。 减少惊讶的一种方法是注意协方差所表示的是：假设您正在使用的模型(有效的A，Q和H，R)是准确的情况下，在进行状态估计时候应该有多不确定。但这并不是说：这就是不确定性。通过明智地调整Q和R，你可以任意改变P。特别是，你不应该把P解释为一个"质量"的数值，而是应该看看观测残差。例如，您可以通过减少R来使P更小。然而，与其计算的sds相比，残差将更大。 当观察值以恒定的速度进入，并且总是同一组观察值时，P将趋于稳定状态，原则上可以提前计算。 但是，当两个观测值之间的时间不同，并且每次都有不同的观测集时，应用卡尔曼滤波器并不困难，例如，如果您有不同采样周期的各种传感器系统。在这种情况下，您将看到P中的更多变化，尽管在原则上，这可以提前计算。 此外，卡尔曼滤波器可以扩展(以各种方式，例如扩展卡尔曼滤波器和无迹卡尔曼滤波器)，以处理非线性动态和非线性观测。在这种情况下，因为转移矩阵(A)和观测模型矩阵(H)具有状态依赖性，所以P也将具有状态依赖性。关于协方差QR： Q代表的是建模误差，也就是预测误差。 --- 越小表示系统越相信预测值 R代表的是测量误差，也就是观测误差。 --- 越小表示系统越相信观测值 **卡尔曼中是用协方差来描述误差和置信度的**，进一步说：协方差影响权重K（这点在K矩阵的定义中已经说明了：求出卡尔曼增益K "*加权后的估计值与系统的真实值之间的误差最小*"。**这一点不难理解：协方差大，说明波动大，说明你状态预测（"理论预测"）或测量预测（"实际测量"）不准、误差大，那我系统自然就对你的信任低，你置信度就低嘛。** **系统接受Q,R后，整个系统实际上已经固定**，**从始至终，系统对状态预测和通过测量预测的相信程度（采纳度）都是相对于实际观测值和状态值静态的,它们只受实际观测值和状态值（提供状态预测和通过测量预测）的波动情况（协方差）影响**。由于系统的这种固定性，在实际应用中传入Q,R时应力求传入准确的协方差矩阵(至少你要告诉卡尔曼滤波小宝宝比较准确的情报吧），以使卡尔曼滤波正常、科学、优质地表现出应有水平。 此外实际测试时，更应关注**相对误差（绝对误差受尺度影响）**，小心地考虑误差到底有多大，不要仅直观观测忽视坐标尺度而被画出来的图所误导。**需要注意问题** 关于是否有帧间时间戳的问题 如果有 单位是啥 这个对应配置参数中的 timestamp_unit: 2 # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond. 这个值没有的情况下 是需要安装扫描方式和频率计算出来的 这个使用的时候要注意 这个值表示的是当前激光点距离dang'q**关键技术：** Fast iterated Kalman filter for odometry optimization; Automaticaly initialized at most steady environments; Parallel KD-Tree Search to decrease the computation;1. KF如何通俗并尽可能详细地解释卡尔曼滤波? - 知乎Kalman 滤波状态矩阵协方差矩阵（描述状态变量之间的关系，比如位置与速度之间）状态转移矩阵（根据当前状态转移得到下一时刻状态矩阵与协方差矩阵）卡尔曼滤波是如何工作的，图解我必须要告诉你一些关于卡尔曼滤波的知识，因为这踏马的太牛B了。奇怪的是很少有软件工程师或者科学家对它有所了解。这让我有点小失望，因为在一些含有不确定因素的场景里，如何去综合获取有效的信息，卡尔曼滤波是一个通用并且强有力的算法。有时候它提取精确信息的能力看上去就像是"见证奇迹的时刻"。如果看到这里你认为我说的话里有夸大的水分，你可以看下我开发的效果视频（http://www.bzarg.com/p/improving-imu-attitude-estimates-with-velocity-data）。在这个demo里我通过检测角速度来获取一个自由物体的姿态，效果奇佳。什么是卡尔曼滤波？你可以在任何含有不确定因素的动态系统里使用卡尔曼滤波，而且你应该可以通过某种数学建模对系统下一步动向做一个大概的预测。尽管系统总是会受到一些未知的干扰，但是卡尔曼滤波总是可以派上用场来提高系统预估的精确度，这样你就可以更加准确地知道到底发生了什么事情(系统状态是如何转移的)。而且它可以有效利用多个粗糙数据之间的关系，而单独面对这些数据你可能都无从下手。卡尔曼滤波尤其适合动态系统。它对于内存要求极低（它仅需要保留系统上一个状态的数据，而不是一段跨度很长的历史数据）。并且它运算很快，这使得它非常适合解决实时问题和应用于嵌入式系统。如果你尝试谷歌去搜索相关资料，对于卡尔曼滤波的数学表达总是很枯燥并且难理解。这提高了大家的学习成本甚至打击了学习兴趣，因为卡尔曼滤波真的是超级简单，当然前提是你用正确的方式去理解它。因此这就形成了一个很有意义的学术话题，我将会通过很多清晰、漂亮的图片以及颜色标注来阐述这个话题。对学习者的预备知识要求很简单，你只需要对概率论和矩阵运算有一些简单的基础知识。我们从一个简单的例子入手，看下卡尔曼滤波可以解决什么问题。如果你想直接看公式推导，可以跳过下一节。利用卡尔曼滤波我们可以做什么？我们举一个玩具的栗子：你开发了一款小型机器人，它可以在树林里自主移动，并且这款机器人需要明确自己的位置以便进行导航。我们可以通过一组状态变量 x¯ 来描述机器人的状态，包括位置和速度：注意这个状态仅仅是系统所有状态中的一部分，你可以选取任何数据变量作为观测的状态。在我们这个例子中选取的是位置和速度，它也可以是水箱中的水位，汽车引擎的温度，一个用户的手指在平板上划过的位置，或者任何你想要跟踪的数据。我们的机器人同时拥有一个GPS传感器，精度在10m。这已经很好了，但是对我们的机器人来说它需要以远高于10m的这个精度来定位自己的位置。在机器人所处的树林里有很多溪谷和断崖，如果机器人对位置误判了哪怕只是几步远的距离，它就有可能掉到坑里。所以仅靠GPS是不够的。同时我们可以获取到一些机器人的运动的信息：驱动轮子的电机指令对我们也有用处。如果没有外界干扰，仅仅是朝一个方向前进，那么下一个时刻的位置只是比上一个时刻的位置在该方向上移动了一个固定距离。当然我们无法获取影响运动的所有信息：机器人可能会受到风力影响，轮子可能会打滑，或者碰到了一些特殊的路况；所以轮子转过的距离并不能完全表示机器人移动的距离，这就导致通过轮子转动预测机器人位置不会非常准确。GPS传感器也会告知我们一些关于机器人状态的信息，但是会包含一些不确定性因素。我们通过轮子转动可以预知机器人是如何运动的，同样也有一定的不准确度。如果我们综合两者的信息呢？可以得到比只依靠单独一个信息来源更精确的结果么？答案当然是YES，这就是卡尔曼滤波要解决的问题。卡尔曼滤波如何看待你的问题我们再来看下需要解决的问题，同样是上边的系统，系统状态包括位置和速度。我们不知道位置和速度的准确值；但是我们可以列出一个准确数值可能落在的区间。在这个范围里，一些数值组合的可能性要高于另一些组合的可能性。卡尔曼滤波假设所有的变量（在我们的例子中为位置和速度）是随机的且符合高斯分布（正态分布）。每个变量有一个平均值ｕ，代表了随机分布的中心值（也表示这是可能性最大的值），和一个方差 σ^2，代表了不确定度。在上图中位置和速度是无关联的，即系统状态中的一个变量并不会告诉你关于另一个变量的任何信息。下图则展示了一些有趣的事情：在现实中，速度和位置是有关联的。如果已经确定位置的值，那么某些速度值存在的可能性更高。假如我们已知上一个状态的位置值，现在要预测下一个状态的位置值。如果我们的速度值很高，我们移动的距离会远一点。相反，如果速度慢，机器人不会走的很远。这种关系在跟踪系统状态时很重要，因为它给了我们更多的信息：一个测量值告诉我们另一个测量值可能是什么样子。这就是卡尔曼滤波的目的，我们要尽量从所有不确定信息中提取有价值的信息！这种关系可以通过一个称作协方差的矩阵表述。简而言之，矩阵中的每个元素 Σij 表示了第i个状态变量和第j个状态变量之间的关系。（你可能猜到了协方差矩阵是对称的，即交换下标i和j并无任何影响）。