关于激光雷达观测及jacobian求导的一些理解一

符号定义

带噪声的激光观测点：

预测的地图坐标点：

真实的激光观测点：

真实的地图坐标点：

预测的pose：

真值的pose：

误差pose：

激光的观测噪声： ~ N(0,)

激光雷达观测的是什么：

激光雷达可以获取一系列3D坐标点，代表着周围物体的表面点距离激光中心的距离和方位。
如果这帧是第一帧，那么就认为这一系列的雷达点，构成了地图的最开始的数据。
如果是接下来的帧，那么就要考虑，如何通过这个帧的一系列激光点，与之前构建的地图点能匹配上，从而计算出当前激光的pose。
有两种方式，一种是点对点，另一种是点到面（或线段）。

点到点的观测

对于点到点来说，就是把激光测量的点，用预测的pose，转换到地图坐标系得到点，找到最近的地图点。如果pose是准确的，那么应当两者要重合，及。

那么观测就是，观测到了地图点，预测就是。即观测量是坐标点。

注意，这个观测到的地图点，实际上是利用了激光的相对坐标，以及预估的pose:，得到了预测的点后，通过最近距离搜索得到的，不是直接获取的。

不像图像，点都是更加稠密的，可以通过特征点方式，来进行独立的匹配。激光不行，不能保证某束激光一定是打到之前的某个地方点，且也没有什么所谓的特征可以用来匹配，这也是这种传感器的特点带来的对于匹配的改变。

这也就会带来一些问题，比如预估的pose比较差，那所谓的观测到的地图点，就不会准确，执行下面的优化，就会越来越差。比如说弱势场景下如直面一堵墙，范围内都是墙，就不能稳定的进行优化。

观测方程

其形式是：

又可以分为以下几种情况：

1、真实的位姿，真实的观测：

 

这个就等于是找到的地图点。

这个我们也不知道它是啥，只知道它与的关系：

2、真实的位姿，带噪声的观测：

这个实际上就是qi附近，呈现v的分布，不知道具体值是多少。

3、预测的位姿，带噪声的观测：

可以计算出来值的。

根据以上的关系，我们可以知道：

  

因为v是0均值的正太分布，正负号无所谓。

表示，就算用真值的pose，计算的地图点，与真实的地图点，之间还是有误差，这个是来源于激光的测量误差。

这个v在后面求偏导中好像没有有什么作用，实际上，反馈了一个在当前误差状态的情况下，观测量的概率分布，就是

 

偏导求取

滤波的做法：

如果是按照滤波的思路来往下走的话，就是要求出观测方程对于误差状态的偏导，即：

这个H有两种求解思路：

1、一种是直接泰勒近似：

那么

这个与优化的求解过程是一样的。

2、另一种是加扰动：

假设：

那么：

就看:与的关系：

需要具体参数化来运算：

                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     

 

优化的做法：

如果是按照优化的思路，是要让残差越来越小，因此要找出残差与误差状态的因果关系，求出残差对于误差状态的偏导：

以上两者，怎么统一理解呢？

实际上，泰勒近似的做法，就是认为，以当前预估的pose，计算出来的预测地图点，之所以与真实的地图坐标点有误差，是因为，pose不准，把这个误差的pose与考虑上，假设它与结果有个变换关系（这个正是我们要求的H），那就能近似得到了。

而残差，本质上就是真实的地图点与预测的地图点的距离，相当于是把泰勒近似的做法，反过来想了一下，即：

变成：

有个误差，这个误差是误差状态量引起的，假设用符合表示这个误差，那就是因果转换矩阵*误差状态量。 这个因果转换矩阵就是偏导。

所以，泰勒近似，与优化的残差与误差量的偏导，是一个事情的两种说法。

加扰动是怎么理解呢？

这个其实是更容易理解的，就是假设pose变动了多少，带来的结果会变化多少。本质上是一回事。

具体做法是列出之前是什么，然后，左边加上一个结果的变化量，右边在变量中增加变量的变化量，最终得到结果的变化量与变量的变化量的关系，然后用极限的方式求出偏导。