一、最小二乘法

1.最小二乘法

2.加权最小二乘法

3.递归最小二乘法

4.最小二乘法与极大似然

二、卡尔曼滤波

1.概述

卡尔曼滤波器与线性递归最小二乘滤波器非常相似。递归最小二乘可以更新参数的估计值，但卡尔曼滤波器却能够对状态量进行估计和更新。卡尔曼滤波器的目标是对该状态量进行概率估计，主要是分两步实时更新：预测和修正。
在这里插入图片描述

首先从时间 k − 1 时的初始概率估计开始，我们使用从车轮里程计或惯性传感器测量得出的数据使用运动模型来预测汽车的位置。
然后，我们使用从 GPS或Lidar 测量得出的数据并使用测量模型来修正 k 时刻汽车的位置。这里，我们可以将卡尔曼滤波器视为一种融合来自不同传感器的信息对未知状态进行最终估计的技术，并同时考虑到运动和测量模型中的不确定性。

2.线性卡尔曼滤波

首先定义汽车的运动模型
$x_{k} = F_{k-1}x_{k-1}+G_{k-1}u_{k-1}+w_{k-1}$
其中 $F_{k-1}$ 是系统状态转移矩阵， $G_{k-1}$ 是系统控制矩阵， $u_{k}$ 是系统输入信号， $w_{k-1}$ 是过程噪声

之后定义测量模型
$y_{k} = H_{k}x_{k}+v_{k}$
其中 $H_{k}$ 是测量矩阵， $v_{k}$ 是测量噪声

卡尔曼滤波的过程如下：
在这里插入图片描述

首先，我们使用运动模型来预测：
$\check{x} _{k} = F_{k-1}x_{k-1}+G_{k-1}u_{k-1}\\ \check{P}_{k}= F_{k-1}\hat{P}_{k-1}F_{k-1}^{T} +Q_{k-1}$
然后，我们计算卡尔曼增益：
$K_{k}=\check{P}_{k}H_{k}^{T}(H_{k}\check{P}_{k}H_{k}^{T}+{R}_{k})^{-1}$
最后，修正系统状态量和协方差矩阵：
$\hat{x} _{k} = \check{x} _{k}+K_{k}(y_{k}-H_{k}\check{x} _{k})\\ \hat{P} _{k} =(I-{K} _{k}{H} _{k})\check{P} _{k}$

在噪声期望为零则不相关的情况下，卡尔曼滤波器是无偏且一致的，即满足以下数学关系：
$E[\hat{e_{k} } ] = 0 \\ E[\hat{e_{k}}\hat{e}_{k}^{T}] = \hat{P_{k}}$
通常，如果我们有白色不相关的零期望噪声，则卡尔曼滤波器是最好的（即最低方差）无偏估计滤波器，它以最小的方差进行无偏估计。

3.扩展卡尔曼滤波

为了对非线性系统进行状态估计，我们将使用称作扩展卡尔曼滤波的状态估计算法。扩展卡尔曼滤波的关键是线性化非线性系统，这里需要用到泰勒展开式。泰勒展开后通常只保留一阶部分，略去高阶部分。

对于非线性系统运动模型和测量模型，我们需要对它们进行线性化处理。我们选择状态的最新估计值作为泰勒展开点，得到如下公式
在这里插入图片描述
对于测量模型，线性化处理为：

在以上公式中， $F_{k-1}$ ， $L_{k-1}$ ， $H_{k}$ ， $M_{k}$ 被称为系统的Jacobian矩阵。Jacobian矩阵是一个包含所有一阶偏导数的矩阵。如下图所示：

将它们代入我们的标准卡尔曼滤波器方程中，含有雅可比矩阵的扩展卡尔曼滤波器方程为：
在这里插入图片描述

Error State卡尔曼滤波

在ES-EKF中，我们将使用EKF估计误差状态，误差状态就是在任何给定时间名义状态（汽车估计位置）和真值状态（汽车真实位置）之间的差异，然后使用误差状态估计值修正名义状态。在运动模型和测量模型中误差状态表示如下：
在这里插入图片描述
ES-EKF的处理方法如下：

首先使用非线性运动模型和当前状态的最佳估计来更新名义状态。
$\check{x_{k}} =f_{k-1}(x_{k-1}, u_{k-1}, 0)$
然后是预测协方差矩阵。
$\check{P_{k}} =F_{k-1}P_{k-1}F_{k-1}^{T}+L_{k-1}Q_{k-1}L_{k-1}^{T}$
当有新的测量值到来时，首先计算卡尔曼增益：
$K_{k}=\check{P_{k}}H_{k}^{T}(H_{k}\check{P_{k}}H_{k}^{T}+R)^{-1}$
然后是计算误差状态。
$\delta \hat{x_{k}} =K_{k}(y_{k}-h_{k}(\check{x_{k}}, 0)$
修正名义状态。
$\hat{x_{k}} = \check{x_{k}}+\delta \hat{x_{k}}$
最后更新协方差矩阵。
$\hat{P_{k}} =(I-K_{k}H_{k})\check{P_{k}}$

EKF的使用限制：

容易产生线性化误差，特别时当系统运动模型和测量模型高度非线性时；或当传感器采样时间过慢时（与自动驾驶汽车速度相比），也会产生很大的线性化误差。
线性化误差很大时，会产生两个结果：一是估计值可能与真实值相差很大，二是估计协方差可能无法准确捕捉状态的真实不确定性，这会导致整个系统发散，而且通常很难再收敛回来。
在实践中最常见的问题是在计算雅可比矩阵时很容易出错，尤其是在处理复杂的非线性系统时。工程上也有人试图通过使用数值微分或在编译时使用自动微分来解决这个问题。但是这些都有自己的陷阱，有时会表现得不可预测。

4.无迹卡尔曼滤波

（1）无迹变换

无迹变换的背后思想是：近似概率分布比近似任意非线性函数更容易。无迹变换的基本思想分为三个步骤。

首先，我们从输入分布中选择一组样本点。这些不是随机样本，它们是确定性样本，它们被选择为远离平均值一定数量的标准差。出于这个原因，这些样本被称为 sigma points。
第二个步骤是将每个 sigma point 通过非线性函数，产生一组属于输出分布的新sigma point。
最后，我们可以使用一些精心选择的权重来计算输出 sigma points的样本均值和协方差，这将使我们能够很好地近似真实输出分布的均值和协方差。

在这里插入图片描述
这其中有两个重要步骤，分别是

选取sigma point：通常，对于n nn维概率分布，我们需要 2 n + 1 2n+12n+1 个 sigma point，一个为均值，其余关于均值对称分布。选取 sigma point 的第一步是对输入分布的协方差矩阵进行 Cholesky分解，我们就可以选择分布均值作为第一个 sigma point，其余 sigma point 的选择可以用图中的公式来实现。其中 κ参数一般取值为3，N为维度。

在这里插入图片描述