前言：本文运用了大量python语言，读不懂没关系，重点在于理解UKF的思想，如何利用概率分布逼近非线性（通过仿真例子理解）。通过对比KF与UKF更容易理解。

引用自：
https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/10-Unscented-Kalman-Filter.ipynb

1.UKF

非线性可以表现在两个方面：量测模型与过程模型
量测：一个雷达，量测对于目标的倾斜长度，取平方根来计算x，y的坐标。

雷达通过发射一束无线电波并通过扫描回波工作。任何物体都会在这束路径上返回一些信息给雷达。通过对于返回雷达所需时间的计算可以计算出倾斜距离（也就是直线距离）。
过程模型：跟踪一个飞行在空中的球，这个球受到重力与空气阻力的作用，因而具有高度非线性。


从输入中生成500000个采样点，通过非线性变换建立结果的分布直方图。
从输出的直方图我们可以计算出均值与标准差，这就是更新值，这些值是近似高斯分布的。
这意味着我们采样的过程就构成了我们问题的解。
—假设每次更新，取500000个点，通过函数变换，再计算均值与方差。
*这叫做马尔科夫方法，它被用来作为一些滤波器设计，例如：Particle 滤波器，Ensemble滤波器。
“足够的点”是非常重要的。
—上图创建了500000个点，输出却仍然不是光滑的。
—得到合理的均值与方差的估计并不需要许多点。
—总的来说，点的数量随着维数呈现指数倍的增加。
如果你只需要50个点用于一维，那么需要2500个点用于二维，以及125000个点用于三维。
无迹卡尔曼滤波使用似然方法，通过一种确定选点的方法极大的减少了计算量。

1.1选择sigma点

让我们看一种2D协方差椭圆的问题。
假设一些任意的非线性函数，我们会选取协方差椭圆中一些随机的点，通过非线性函数变化并画出他们新的位置。


左侧显示的是两个状态变量1sigma分布的椭圆。
箭头表示的是三个随机选取的采样点通过非线性函数生成的新的分布。
右侧的椭圆就是这些点的集合的均值与方差的估计。
—如果我们要去采样，说选取一百万个点，这些点的形状可能与椭圆相差很远。
让我们通过非线性函数跑一些点进行观察。

均值与协方差为：



这个图表明了对于强非线性的函数，如果我们在（0，0）点对其进行线性化会导致很大的误差，这样的滤波器叫做扩展卡尔曼滤波。
虽然选取50000个点进行均值计算非常精确，但是计算50000个点会使我们的滤波器更新非常的慢。
我们能使用的最少的采样点是什么？
制定的问题对于点的选择有哪些约束？
让我们思考可能的最简单的问题并观察能否得到一些新的认识。可能的最简单的系统的独特的—变换并不改变输入。用数学符号表示就是f(x)=x，我们可以用的最少的点就是每维一个点。这是线性卡尔曼滤波所使用的。对于分布为正态分布的卡尔曼滤波这个输入就是均值u。
如果我们使用u+△输入进特定的函数f(x)=x，它不会收敛，因为这是不可能的算法。（这个我不太理解什么意思）
所以什么是我们可以选择的次最小的数目？思考高斯分布的对称性，次最小的数目是三个点，一个点是均值，另外两个分布在均值两侧，就如下图所示。

为了使其有效，我们希望权重之和等于1。
—如果总和大于或小于1，则采样将不会产生正确的输出。
鉴于此，我们必须选择sigma点X及其相应的权重，以便计算输入高斯的均值和方差。这并不是唯一的答案—这个问题不受约束。
—例如：如果你选择一个小的权重对于均值这个点，你将通过选择大的权值对于其他的点以用来补偿。（因为总的为1）
—这些等式不要求任何点都是输入的均值，这来起来是‘不错’的。
我们选择三个点对于每个维度从协方差中。
这选择是完全确定的。
下面是三种不同的例子对于相同的协方差椭圆。上述的约束条件不要求沿着椭圆的长轴与短轴分布，但是这样做很典型。
此外，在每种情况下这些点等间隔分布，这也不是约束条件要求的。但是这是一个合理的选择，毕竟如果我们想要准确的采样输入，采用对称的方法采样是有意义的。
有几种公认的选择sigma点的方法。
现在我们会使用Julier and Uhlmann的原始的实现方法，定义了常数k用来控制sigma点的分离程度。<选自于> Julier, Simon J. Uhlmann,Jeffrey“A New Extension of the Kalman Filter to Nonlinear Systems”. Proc. SPIE 3068, SignalProcessing, Sensor Fusion, and Target Recognition VI, 182 (July 28, 1997)
在显示第一个和第二个偏差的协方差椭圆顶部绘制sigma点，并根据分配给它们的权重确定它们的大小。●sigma点位于第一个方差和第二个方差之间，并且越大的k使这些点分离的越远。
较大的k表示比较小的k在均值这个点的权重大，其余的sigma的权值会相应变小（因为总和为1）。
—这也是符合我们的直观感受的，距离平均值越远，我们应该加权越小。

1.2非线性处理

这里有两个地方会出现非线性：
—状态转移函数
—量测函数
线性kalman滤波
—使用公式

进行状态的一步预测。
F定义了进行计算的一组线性方程
—使用量测函数是

将量测值合并到状态中（即通过量测值修正状态值，但是某些情况下并不是直接的，所以此处用词是incorporate，合并）
*Hx转换x状态为一个量测值通过H线性观测方程。
标准UFK假设量测函数与状态转移函数全都是非线性的，虽然在一些问题中只有一个是非线性的。
定义函数h(x)为量测函数，f(x)为状态转移函数。

1.3 sigma点算法

第一个sigma点是输入的均值。

相应的权值为：

其中n是问题的维度，也就是状态的维度，k是缩放因子。
剩下的sigma点定义为：

相应的权值为：

k是缩放因子，控制sigma点与均值的分离距离。
—较大的k会选择远离均值的sigma点，反之较小的k会选择离均值近的点。
Julier and Uhlmann建议使用k+n=3，当分布是高斯时，或许当不是高斯时应该选择不同的值。
—为什么这是对的，这似乎不是很明显，当然，这对于非线性问题是理想的条件也很难被证明。
—我们选择sigma点是与协方差矩阵的平方根成比例的，方差的平方根是标准差。因此，sigma点是大致根据1标准差分布的。
—这里存在一个n项-（n是维数）越多的维数需要越多的点，这些点分摊开，每个点的权重也会变小。

1.4无迹变换

无迹变换是算法的核心。
给定一组sigma点和一个传递函数，我们需要计算新的均值与协方差（经过这个函数变换后的）。注解：传递函数指的是状态传递函数，即f(x)。
—使sigma点不断的通过非线性函数的计算得到其新的位置
—计算新的位置构成的协方差与均值

1.5无迹滤波器

假设存在一个状态转移函数
—由给定的当前时刻的状态预测下个时刻的状态
假设存在一个量测函数h（x）
—通过当前的状态得到当前状态所对应的量测

1.5.1预测步骤

与线性kalman滤波一样，UKF采用过程模型对下一时刻的状态均值与协方差进行预测。
首先，生成具体的sigma点与其对应的权重

将sigma点通过函数f(x)
—这将使sigma点通过过程模型向前投影，也就相当于向前预测一步。

使用无迹变换得到的并且经过状态转移的sigma点计算预测均值与协方差。

计算的均值与协方差由下图中的绿色椭圆所表示，对应线性kalman的公式：

从图像来看，这看起来像“箭头表明使sigma点通过非线性函数f的效果”，绿色的椭圆是由无迹变换计算出的均值与协方差。

1.5.2 更新步骤

将sigma点从预测步骤转换为量测使用函数h(x)

使用无迹变换计算这些点的均值与协方差（在预测步骤用是用无迹变换计算下一时刻状态的均值与协方差，而这里是计算对应的量测的均值与协方差）
下标z表明是量测的均值与协方差
计算残差与Kalman增益

使用残差与kalman增益计算新的状态估计
计算新的协方差

1.5.3 对比KF公式与UKF公式

1.6使用UKF

虽然UKF是为非线性问题设计的，但是它对于线性问题也能很好的适应。
使用二维恒速模型编写求解线性问题的求解器。
首先，设计线性kalman滤波器参数x,F,H,R,Q。

状态转移模型

由牛顿公式

得到。
量测函数

量测噪声矩阵

过程噪声



现在，使用UKF滤波器代替线性KF滤波器，仅仅是为了教学举例，使用UKF解决线性问题不是有益的。
确定非线性函数矩阵f(x)与h(x)
f(x)是状态转移函数，在线性滤波器中由F矩阵实现。
h(x)是量测函数由H矩阵实现。
对于我们的线性问题，我们可以定义这些函数实现线性方程f(x)记作f(x,dt)，h(x)记作h(x,dt)。


UKF标准差0.013
在这个例子中，KF与UKF差别不大
—我们仅仅是使用F替代了f(x)，H替代了h(x)。
—其他的条件均没有改变。

1.7追踪飞机

1.7.1初次尝试

写一个滤波器使用静态雷达作为传感器在二维平面追踪飞机。
—追踪飞机在地平面的一个维度与飞机高度。
—使用雷达量测到的斜距计算水平位置（雷达到飞机距离在地平面的投影）以及飞机的高度。


假设飞机的高度是固定的，那么这三个状态向量会有作用。

状态转移函数是线性的


设计量测函数。将滤波器状态转换为雷达的量测，方位与斜距。
使用x,y坐标代表雷达的位置，则可以直接计算出方位与斜距

计算方位我们需要反三角函数：



假设斜距的精度为5m，角度的精度为0.5°，飞机位于雷达的上方以100m/s的速度离开雷达。
典型的雷达的每12秒更新一次，我们用12秒作为更新周期。

1.7.2跟踪机动飞机

先前的例子产生的结果都极好，这是依赖于假设飞机恒定速度以及恒定高度飞行。
下面，解除这个假设，允许飞机改变高度。
—首先我们来看之前的代码在飞机一分钟后开始爬升的情况下的表现。
实际高度：2561.9
UKF高度：1097.9
性能表现是极差的，滤波器完全不能跟踪上高度的变化。
将状态中增加爬升率。

这需要状态转移方程改变，但仍然是线性的。

量测方程也是同样的改动，另外Q也要考虑到状态维度的变化而改动。

实际高度：2561.9
UKF高度：2432.9
我们发现虽然引入了高度噪声，但是还是能够准确的跟踪高度变化。

1.7.3传感器融合

考虑一个传感器融合简单的例子。
假设我们拥有几种多普勒类型的系统可以以2m/s RMS的精度进行速度估计。
降低雷达的精度为了更明显的看出传感器融合的效果。Remark：（融合的是速度传感器，降低雷达的斜距传感可以减少距离信息对速度信息的修正，从而使速度传感器融合与否得到的结果对比更加明显）。
—将斜距的误差改为500m，计算估计出的速度的标准差。

通过对速度传感器的结合我们使速度的标准差从3.5m/s降低到了1.3m/s。

1.7.4多位置传感器

在带GPS的船舶和飞机出现之前，需要通过各种范围和方位系统（例如VOR，LORAN，TACAN，DME等）进行导航。 通常，这些系统是从信标提取距离和方位，或者是距离和方位的信标。
例如，一架飞机可能有两个VOR（高频全方位导航系统）接收器。 飞行员将每个接收机调谐到不同的VOR电台。 每个VOR接收器都会显示所谓的“径向”，即从地面VOR站到飞机的方向。使用图表显示，他们可以扩展为这两个径向线的相交点是飞机的位置。这是一种手动且精度不高的方法，我们可以使用卡尔曼滤波器来过滤数据并产生更加准确的位置估算值。
假设我们有两个传感器，每个传感器仅提供对目标的方位测量，如以下图表。 在图表中，圆圈的宽度旨在表示不同数量的传感器噪声。


我们可以计算传感器与目标的方位角，如下：
def bearing(sensor, target): return math.atan2(target[1]- sensor[1], target[0] - sensor[0])
滤波器可以得到两个量测的向量在每次更新周期内，每个传感器一个。
def measurement(A pos, B_pos, pos)：anglea = bearing(A_pos, pos) angle_b = bearing(B_pos, pos)return [angle. a, angle_b]
测量函数和状态转换函数的设计可以保持不变，因为没有任何改变会影响它们。
这看起来是相当好的。刚开始的时候有很大的追踪误差，但是滤波器能够稳定然后估计出正确的数据。
让我们重新审视角度的非线性。
将目标定位在两个传感器之间连线上或具有相同y坐标的左侧时，将导致sigma均值和残差的计算出现非线性。当角度在pi附近时，测量读数将为正值大约pi的角度或-pi的负角度。
平均角度（预测）将接近零。因此，预测和测量之间的残差将接近π或一π，而不是接近0。这使得滤波器无法准确运行，如下例所示。
对于现实世界的滤波器来说，这是站不住脚的行为。 FilterPy的UKF代码可以为它提供一个函数，以在这种非线性行为的情况下计算残差。def update(self, z, R=None, residual=np.subtract, UT=None):””使用给定的测量值更新UKF。 返回self.x和self.P包含滤波器的新均值和协方差。

1.8追踪圆周运动的目标

改变仿真中目标的运动方式，改为圆周运动。通过将传感器放在目标运动圆周的外部避免角度非线性（角度为0度或者180度）。
如果我们知道目标的运动，我们可以设计出滤波器的状态转移函数，以至于我们可以预测物体的圆周运动。但是这里我们不会在滤波器中建立有关运动的先验知识。

1.8.1讨论

滤波器追踪了目标的运动，但是并没有收敛到目标的轨迹上。
这是因为滤波器建立的是目标的恒定速度模型，但是目标并不是恒定速度（因为目标速度的方向在变）。
通过上述提到的问题，我们可以建立圆周运动的模型通过定义f(x)函数，但是这时我们又将面临的问题是当目标不再做圆周运动了怎么办？
另外，我们可以通过增加Q的值从0.1到1来告诉滤波器我们更加不确定过程模型是否准确。

这个并不是完美收敛，但是已经好多了。

1.9计算位置误差

滤波器的位置误差非常依赖于目标距离传感器的远近。写一个根据方位误差计算距离误差的函数。

1.9.1讨论

我们可以看出非常小的方位误差也可能转化为非常大的距离误差。
更糟糕的是，当运动非线性时，x轴与y轴的误差更加依赖于准确的方位角。
例如：这里是一个散点图，显示了方位差的标准差为1度的情况下方位角为30度时的误差分布。

1.10解释滤波器性能

我们可以看到即使是非常小的角度误差也会有非常大的（x,y）的位置误差。阐述如何能够获得相对好的性能针对上述的跟踪问题。
这里有几点成功的原因。
首先，让我们考虑只有一种传感器的情况。
—虽然任何一次量测都存在可能的位置有着极长的斜距，但是目标是移动的，UKF也将其考虑在内。
—让我们绘制连续运动目标的数次量测从而形成直观的认识。

每个独立的量测有非常大的位置误差。然而，当我们成功的绘制了所有的量测后，我们可以观察到运动的趋势是非常明显的向右上方移动。
当kalman滤波器计算kalman增益时，它通过使用量测函数将误差的分布考虑在内。
—在此示例中，误差位于大约45度的直线上，因此过滤器将消除该方向上的误差。
—另一方面，正交于此的测量几乎没有误差，并且卡尔曼增益会再次将其考虑在内。
—-最终结果是即使使用这种噪声分布也可以计算轨迹。
这个图中很容易看出物体运动的方向，因为我们计算了每次位置更新对应的100个可能的量测，绘制出了他们全部的图像。相对的，kalman滤波器每次更新只获得一次量测，所以不能得到像绿点构成的直线显示的那样好的结果。
现在，让我们考虑增加传感器来带的效果。同样，首先绘制量测使我们对于问题有直观的认识。
传感器对于目标初始位置是近似正交放置的，所以我们可以得到可爱的“x”形状的相交部分。
然后计算对应于两个含有噪声方位测量值的（x，y）坐标，并用红点绘制它们，以显示带噪声的测量值在x和y中的分布。
—我们可以看到x和y的误差是如何随着目标移动而变化的，随着目标远离传感器目标的移动通过红色的散点图表现。但是，当目标越接近于B传感器（即右侧）的y坐标时，红色的散点图越呈现椭圆形状。
下面改变初始位置进行仿真，在此，x和y中误差的形状会发生根本变化随着目标相对于两个传感器位置的变化。

1.11可视化sigma点

使用UKF生成sigma点并且使他们通过非线性函数得到输出并绘制它们的图像与5000个sigma点的均值。
—sigma点通过的状态传递函数为：

均值为：

协方差为：

1.11.1讨论

第一次训练y轴的误差大约在43（这句确实不太理解说的是什么），通过50000个点的UKF仿真得到的误差在0.2左右。

1.12UKF的实现

学习如何将公式转化为代码是由启发性的。另外，如果你遇到问题需要调试你的代码，你可能需要使用调试器一步一步的执行你的代码。
由sigma点计算均值与协方差
—存储sigma点与权值矩阵

每列包含2n +1个sigma点的一维。序号为0的sigma点总是均值，所以第一行的sigma点包含所有维度的均值。第二行到第n行是包含
项，第n+1到2n+1是包含项。

1.12.1权重

回忆Julier实现

np.full()创建具有相同值的数组。（python函数）然后计算均值W0和重写填充的数值。

1.12.2sigma点

Sigma点的公式是：

含有协方差的项是矩阵项，因为协方差为矩阵，下标i是选择矩阵的第i列。矩阵的平方根是什么？通常对于矩阵平方根的定义为S乘以它自身得到协方差矩阵即S为协方差矩阵平方根。

然而这里还有可以其他选择的定义，我们选择上述定义是因为它的数值性质使得它计算起来比较简单。我们选择矩阵S乘以它的转置得到协方差矩阵，定义S为协方差矩阵平方根。

后一种方法通常在计算线性代数中选择，因为此表达式易于使用称为Cholesky分解的方法进行计算。NumPy为此提供了numpy .linalg. cholesky（）方法。 Matlab提供chol（）,做同样的事情(进行cholesky分解)。
此方法返回一个较低的三角矩阵(这里lower不太理解)，因此我们将对其进行转置，以便在我们的for循环中，可以按U [i]逐行访问它，而不是更麻烦的用按列表示法U [i ,:]。


这引入了NumPy的另一个功能。状态变量X是一维的，同样也是； Xi [k]与X的差也是一维的。NumPy不会计算一维数组的转置。它认为[1,2,3]的转置为[1,2,3]。所以我们调用np,outer(y,y)用来计算y乘以y的转置的数值。
Sigma点函数可由以下代码实现：

1.12.3预测步骤

对于预测步骤，我们和上面一样生成sigma点与权重。使sigma点通过函数f（x）。

然后通过无迹变换计算预测的均值与协方差。在下面的代码中，你可以看到我定义了一个储存了滤波器所需要的变量矩阵与向量的函数。

1.12.4更新步骤

更新步骤通过h(x)将sigma点转换到量测空间。

通过无迹变换计算均值与协方差，同时计算kalman增益与残差。互协方差由下式计算

最后由kalman增益计算新的状态估计

新的协方差计算

这个函数可以由下面代码实现，假设这里存在函数储存着需要的矩阵与数据。

1.13选择kappa值

关于选择kappa值的文献非常少。
如果是高斯分布的话，Julier最初的文章建议选择n+kappa的值等于3，如果不是高斯分布，那么可能选择其他的值是更合适的，它把kappa描述为“微调”的参数。
所以我们来探究它的作用，令n=1，最小化我们需要观察的数组大小。

对于均值为0的算法，我们选择sigma点为0，3，-3，为什么呢？回忆计算sigma点的公式

这里由于n=1，所以矩阵都退化为了标量，避免了我们计算矩阵的平方根，所以我们计算得：

所以当kappa越大时sigma点越分散。让我们看一个比较荒唐（极端）的值。

Sigma的分布开始由含有kappa的项占据主导地位。这里n+kappa=201代替了Julier提出的n+kappa=3。
—如果我们的数据是高斯数据，那么我们将融合许多远离均值的偏差； 对于非线性问题，这不太可能产生良好的结果。
—假设我们的分布不是高斯分布，而是厚尾分布。 我们可能需要从这些尾巴取样得到一个很好的估计，因此将kappa增大是有意义的。
—假设我们的分布几乎没有尾巴的概率分布，看起来更像是倒抛物线。 在这种情况下，我们可能希望将sigma点接近平均值，以避免在永远不会有真实数据的区域中进行采样。
现在让我们观察权重的变化。
当我们使k + n = 3时，均值处的权重为0.6667，两个独立的sigma点的权重为0.1667。
另一方面，当kappa= 200时，均值处的权重上升到0.995，而独立sigma点的权重为0.0025。 回忆一下权重的等式：

我们可以得出：
随着kappa变大，均值的权重接近1，其余sigma点的权重接近0。
协方差大小是不变的。
因此，随着我们的采样与均值的距离越来越远，最终给那些样本的权重降低了，相反，如果我们非常接近均值的采样，那么所有样本的权重都将非常相似。

翻译自https://blog.csdn.net/qq_41011336/article/details/84401835