最近在细读SEBASTIAN THRUN大神的Probabilistic Robotics。
无知的我现在才知道有这么一本神书。很多不懂的细节都在书里面有详细的说明。
相见恨晚+惭愧
在这里，会不定期更新学习笔记。（文章内容图片基本都来自上述的书）
今天写一写泰勒展开式在EKF（Extended KF）的作用。
为什么要用泰勒展开式？
首先，KF 是处理线性问题的基于贝叶斯滤波的滤波器。
KF本来就是为了处理线性高斯系统的滤波器。
用KF的基本条件就是，接受了马尔科夫假设和噪声是满足高斯分布的假设。
或者换句话说就是，只有接受了贝叶斯滤波的马尔科夫假设和噪声是满足高斯分布的假设， KF才会产生效果。当然没有这个假设，KF会运行，但是没有效果罢了。
KF的5大公式中的理念就是，置信度利用均值和方差表示。像粒子滤波器之类的，就不需要特别的均值和方差等参数去表示噪声的特性，所以也叫非参数滤波器
那么，话说回来，为了处理非线性的问题，从KF 衍生出了EKF。
EKF的基本形态
基本的KF：
x= Ax_{t-1}+ Bu_t + \varepsilon_t
z =Cx_t + \delta_t
对比基本的KF，可以看出，EKF把 Ax_{t-1}+ Bu_t \ 的部分用g(u_t,x_t-1) 来代替， Cx_t 也是一样用 h(x_t) 代替。 那么这里表现的含义是？
-状态转移方程不再是线性的了。
那么不线性对使用KF有啥影响？
KF对噪声的假设就是，噪声一定要属于正态分布。也就是说，对于一个状态来说，不停的采样N次的时候，这个状态的分布应该是钟型曲线。KF的状态转移又是线性的。那么正态分布经的状态经过线性变化之后，出现的分布图其实还是线性的。 如下图（a）。
但是当状态转移方程不再是线性了， 那么经过非线性变换的状态的分布就不再属于正态分布了。如下图（b）。
KF和EKF的根本问题
那么KF的理论的基本假设（噪声是正态分布）就会崩塌。为了满足KF的基本假设，就需要一种方法把非线性的状态转移方程转化为线性方程。
他就是， 泰勒展开式 （其实泰勒展开式东西我们初中高中好像就学过，一直不知道有啥用）
泰勒公式
泰勒公式的作用就是，当求一个非线性的f(x)很困难的时候，可以利用这个x的1,2......N阶导数来近似这个值。（感觉也是初高中数学。。）
所以利用泰勒公式的特性对g，h函数进行求导数，用此方法来近似实际的值。注意： 如果g，h的函数的参数多，那么为了找出每个参数（状态）对函数的影响，则需要对每个参数（状态）进行求偏导数。
这里利用的是一阶泰勒展开式。
那么泰勒公式里面的x是啥？在机器人或者无人车的定位里面，x就是state。x可以是pose， 可以是position。反正x只是一个状态或者一堆状态。
但需要注意的是，x不是一个特定的值。（个人觉得这个概念很重要，因为一开始的总会把x当做一个值，所以一直不明白 \mu 到底是啥东西）
x是有特定的概率密度函数下随机生成的值。但是，我们需要参数去表示p（x）。
所以一般利用平均值 \mu 和方差 \sigma^2 。
那么回到EKF继续说明泰勒展开的作用。
非线性的函数g（x）如果没有做过任何处理的话，会生成下图中的左上图中的灰色的概率分布。可能他有一定的规律，或者没有~ 但是能明显看出来的内容就是，他的分布不是正态分布。
对非线性的g（x）发动技能：泰勒展开！
非线性的g（x）被强制转化为有正态分布特性的线性方程。泰勒展开式的\mu是下图（右下）p(x）的平均值。
对x-1进行泰勒展开
利用导数求近似的时候，注意是只对xt-1求偏导
利用泰勒一级展开就是如下。
根据状态转移方程prediction阶段的泰勒展开
这里求出的 g(u_t,x_t-1) 是x的预测值，标记为 \bar{x} 。
\bar{x}求出来之后，利用多变量正态分布概率密度函数，求x的平均值和方差。
xt和 xt_预测值的多变量正态分布函数
那么这个时间节点上， x_t 就可以利用平均值和方差来表示。
R_t 是方差。什么的方差？
x和之前求出来的 \bar x 的方差。但是 R_t 数值上应该等于什么？据我以前的知识，是状态方程本身的噪声的的方差。（但是现在还不确定，以后弄明白了再更新。)
G_t 是状态转移方程的偏导数。如果state 是vector， 那么 G_t 就是一个矩阵。这矩阵叫Jacobian 矩阵，雅克比矩阵。
但是x的置信度还是个问题。
所以通过 p(z_t,x_t) 来验证x的置信度。(也就是bayesian 理论）
计算p(z_t,x_t) 其实就是计算z_t，x_t 这两个随机变量的概率分布。 也就是说，他们可以利用多变量正态分布概率密度函数来求解.
多变量正态分布概率密度函数
多变量正态分布概率密度函数的别的形态
（假设测量方程也是非线性的）就像对状态转移方程进行泰勒展开一样，对非线性的测量方程，也要进行泰勒一级展开。跟上面状态转移方程的展开式同一个意义，故不再解释。
测量方程泰勒展开
将 z_t, x_t （2个变量）代入多变量正态分布概率密度函数中，可以得到下面的公式。
那么，作为上面这个公式输入的的Qt是什么？Ht是什么？
Q_t 是方差。什么的方差？测量方程本身的系统方差。
H_t 是测量方程的偏导数。如果state 是vector， 那么 H_t 就是一个矩阵。这个矩阵叫Jacobian 矩阵，雅克比矩阵。
写了一点关于泰勒展开式在EKF的作用。
还需解决的问题：
KF系列的滤波器中，协方差到底是起什么作用的？
EKF线性化的部分因为是近似，但是我们从本文第一张图的（b）的左上图可以看出，明显EKF为了线性化而放弃了精准度。那么有什么更好的方法去拟合非线性的状态（测量）方程呢？
继续学习，下次再更新~