从最小二乘法的角度来理解卡尔曼滤波（1）

flyfish

假设你有一堆数据点，比如在一个二维平面上有很多点。你想找到一条直线，能够尽可能接近这些点。这条直线可以用一个方程来表示：y = mx + b，其中 m 是斜率，b 是截距。

但是这些数据点通常不会完美地在这条直线上，而是散布在它的周围。每个点与这条直线之间的距离称为误差（残差）。为了找到最佳的直线，我们希望所有这些误差的平方和最小。为什么是平方和？因为这样可以避免正负误差相互抵消。

最小二乘法就是通过调整 m 和 b 来使得所有点到直线的距离的平方和最小。

数学公式

假设我们有 $n$ 个数据点 $(x_1,y_1),(x_2,y_2),...,(x_n,y_n)$，我们的目标是找到直线 $y=mx+b$ 使得以下的平方和最小：

$$\text{SSE}=\sum _{i=1}^n(y_i-(m{x}_i+b){)}^2$$

其中，SSE 是残差平方和（Sum of Squared Errors）。
演示下样子

import numpy as np
import matplotlib.pyplot as plt

# 生成一些示例数据
np.random.seed(0)
x = np.linspace(0, 10, 100)
y = 2.5 * x + np.random.normal(size=x.size)

# 最小二乘法计算斜率和截距
def least_squares(x, y):
    n = len(x)
    x_mean = np.mean(x)
    y_mean = np.mean(y)
    xy_mean = np.mean(x * y)
    xx_mean = np.mean(x * x)

    m = (xy_mean - x_mean * y_mean) / (xx_mean - x_mean * x_mean)
    b = y_mean - m * x_mean

    return m, b

m, b = least_squares(x, y)

# 绘制数据点和拟合直线
plt.scatter(x, y, label='Data Points')
plt.plot(x, m * x + b, color='red', label=f'Fitted Line: y = {m:.2f}x + {b:.2f}')
plt.xlabel('x')
plt.ylabel('y')
plt.legend()
plt.title('Least Squares Fitting')
plt.show()

最小二乘法的解计算过程

我们通过最小化残差平方和 $\text{SSE}={\sum }_{i=1}^n(y_i-(m{x}_i+b){)}^2$ 来求解最佳的直线拟合参数 $m$ 和 $b$。下面是详细的计算步骤。

1. 残差平方和公式

首先，残差平方和的公式如下：

$$\text{SSE}=\sum _{i=1}^n(y_i-(m{x}_i+b){)}^2$$

2. 对 $b$ 和 $m$ 求导并设导数为零

分别对 $b$ 和 $m$ 求导并设导数为零，得到两个方程：

$$\frac{\partial \text{SSE}}{\partial b}=-2\sum _{i=1}^n(y_i-(m{x}_i+b))=0$$
$$\frac{\partial \text{SSE}}{\partial m}=-2\sum _{i=1}^n{x}_i(y_i-(m{x}_i+b))=0$$

简化上面的两个方程，我们得到：

${\sum }_{i=1}^n{y}_i=m{\sum }_{i=1}^n{x}_i+nb$
${\sum }_{i=1}^n{x}_i{y}_i=m{\sum }_{i=1}^n{x}_i^2+b{\sum }_{i=1}^n{x}_i$

3. 求解 $m$ 和 $b$

首先，通过代数操作解出 $b$：

$$b=\frac{{\sum }_{i=1}^n{y}_i-m{\sum }_{i=1}^n{x}_i}{n}$$

然后，将 $b$ 代入到第二个方程中，解出 $m$：

$$m=\frac{n{\sum }_{i=1}^n{x}_i{y}_i-{\sum }_{i=1}^n{x}_i{\sum }_{i=1}^n{y}_i}{n{\sum }_{i=1}^n{x}_i^2-({\sum }_{i=1}^n{x}_i{)}^2}$$

最后，通过计算 $m$ 的值，可以求出 $b$ 的值。

最小二乘法中的导数求解与简化过程

1. 残差平方和公式

首先，我们从残差平方和公式开始：

$$\text{SSE}=\sum _{i=1}^n(y_i-(m{x}_i+b){)}^2$$

2. 对 $b$ 和 $m$ 求导并设导数为零

为了找到最小值，我们需要对 $b$ 和 $m$ 求导数，并将导数设为零。

对 $b$ 求导：

$$\frac{\partial \text{SSE}}{\partial b}=\frac{\partial }{\partial b}\sum _{i=1}^n(y_i-(m{x}_i+b){)}^2$$

我们将求导数的过程分步展开：

$$\frac{\partial \text{SSE}}{\partial b}=\sum _{i=1}^n\frac{\partial }{\partial b}(y_i-(m{x}_i+b){)}^2$$

使用链式法则：

$$\frac{\partial }{\partial b}(y_i-(m{x}_i+b){)}^2=2(y_i-(m{x}_i+b))\cdot \frac{\partial }{\partial b}(y_i-(m{x}_i+b))$$

注意到 $\frac{\partial }{\partial b}(y_i-(m{x}_i+b))=-1$，所以：

$$\frac{\partial \text{SSE}}{\partial b}=\sum _{i=1}^{n}2(y_i-(m{x}_i+b))(-1)=-2\sum _{i=1}^n(y_i-(m{x}_i+b))$$

令其为零：

$$-2\sum _{i=1}^n(y_i-(m{x}_i+b))=0$$

简化得到：

$$\sum _{i=1}^n(y_i-m{x}_i-b)=0$$

对 $m$ 求导：

$$\frac{\partial \text{SSE}}{\partial m}=\frac{\partial }{\partial m}\sum _{i=1}^n(y_i-(m{x}_i+b){)}^2$$

同样地，我们分步展开：

$$\frac{\partial \text{SSE}}{\partial m}=\sum _{i=1}^n\frac{\partial }{\partial m}(y_i-(m{x}_i+b){)}^2$$

使用链式法则：

$$\frac{\partial }{\partial m}(y_i-(m{x}_i+b){)}^2=2(y_i-(m{x}_i+b))\cdot \frac{\partial }{\partial m}(y_i-(m{x}_i+b))$$

注意到 $\frac{\partial }{\partial m}(y_i-(m{x}_i+b))=-x_i$，所以：

$\frac{\partial \text{SSE}}{\partial m}={\sum }_{i=1}^{n}2(y_i-(m{x}_i+b))(-x_i)=-2{\sum }_{i=1}^n{x}_i(y_i-(m{x}_i+b))$

令其为零：

$$-2\sum _{i=1}^n{x}_i(y_i-(m{x}_i+b))=0$$

简化得到：

$$\sum _{i=1}^n{x}_i(y_i-m{x}_i-b)=0$$

3. 简化方程

对 $b$ 的方程：

$$\sum _{i=1}^n(y_i-m{x}_i-b)=0$$

拆开括号：

$$\sum _{i=1}^n{y}_i-m\sum _{i=1}^n{x}_i-nb=0$$

我们得到第一个方程：

$$\sum _{i=1}^n{y}_i=m\sum _{i=1}^n{x}_i+nb\text{\hspace{1em}(1)}$$

对 $m$ 的方程：

$$\sum _{i=1}^n{x}_i(y_i-m{x}_i-b)=0$$

拆开括号：

$$\sum _{i=1}^n{x}_i{y}_i-m\sum _{i=1}^n{x}_i^2-b\sum _{i=1}^n{x}_i=0$$

我们得到第二个方程：

$$\sum _{i=1}^n{x}_i{y}_i=m\sum _{i=1}^n{x}_i^2+b\sum _{i=1}^n{x}_i\text{\hspace{1em}(2)}$$

4. 联立方程求解 $m$ 和 $b$

从方程（1）中解出 $b$：

$$b=\frac{{\sum }_{i=1}^n{y}_i-m{\sum }_{i=1}^n{x}_i}{n}$$

将 $b$ 代入方程（2）：

$$\sum _{i=1}^n{x}_i{y}_i=m\sum _{i=1}^n{x}_i^2+\left(\frac{{\sum }_{i=1}^n{y}_i-m{\sum }_{i=1}^n{x}_i}{n}\right)\sum _{i=1}^n{x}_i$$

简化：

$$\sum _{i=1}^n{x}_i{y}_i=m\sum _{i=1}^n{x}_i^2+\frac{{\sum }_{i=1}^n{y}_i{\sum }_{i=1}^n{x}_i-m({\sum }_{i=1}^n{x}_i{)}^2}{n}$$

乘以 $n$：

$$n\sum _{i=1}^n{x}_i{y}_i=mn\sum _{i=1}^n{x}_i^2+\sum _{i=1}^n{y}_i\sum _{i=1}^n{x}_i-m(\sum _{i=1}^n{x}_i{)}^2$$

合并含 $m$ 的项：

$$n\sum _{i=1}^n{x}_i{y}_i-\sum _{i=1}^n{y}_i\sum _{i=1}^n{x}_i=m\left(n\sum _{i=1}^n{x}_i^2-(\sum _{i=1}^n{x}_i{)}^2\right)$$

解出 $m$：

$$m=\frac{n{\sum }_{i=1}^n{x}_i{y}_i-{\sum }_{i=1}^n{x}_i{\sum }_{i=1}^n{y}_i}{n{\sum }_{i=1}^n{x}_i^2-({\sum }_{i=1}^n{x}_i{)}^2}$$

将 $m$ 代入方程（1）求 $b$：

$$b=\frac{{\sum }_{i=1}^n{y}_i-m{\sum }_{i=1}^n{x}_i}{n}$$

链式法则（Chain Rule）

链式法则是微积分中的一个重要法则，它用于求复合函数的导数。复合函数是指一个函数的值依赖于另一个函数的值，例如 $f(g(x))$。链式法则允许我们将复合函数的导数分解为内部函数和外部函数的导数之积。

链式法则的数学表达

设 $h(x)=f(g(x))$，其中 $y=g(x)$ 和 $z=f(y)$。那么，链式法则的数学表达为：

$\frac{d}{dx}h(x)=\frac{d}{dx}f(g(x))=f^{\prime }(g(x))\cdot g^{\prime }(x)$

即：

$\frac{dz}{dx}=\frac{dz}{dy}\cdot \frac{dy}{dx}$

对 $b$ 求导

我们有残差平方和的公式：

$\text{SSE}={\sum }_{i=1}^n(y_i-(m{x}_i+b){)}^2$

对 $b$ 求导时，我们可以看到 $(y_i-(m{x}_i+b){)}^2$ 是一个复合函数，其中 $y_i-(m{x}_i+b)$ 是内部函数，平方是外部函数。应用链式法则：

$\frac{\partial }{\partial b}(y_i-(m{x}_i+b){)}^2=2(y_i-(m{x}_i+b))\cdot \frac{\partial }{\partial b}(y_i-(m{x}_i+b))$

这里 $\frac{\partial }{\partial b}(y_i-(m{x}_i+b))=-1$，所以：

$\frac{\partial }{\partial b}(y_i-(m{x}_i+b){)}^2=2(y_i-(m{x}_i+b))\cdot (-1)=-2(y_i-(m{x}_i+b))$

对 $m$ 求导

同样地，对 $m$ 求导时，我们处理的也是一个复合函数：

$\frac{\partial }{\partial m}(y_i-(m{x}_i+b){)}^2=2(y_i-(m{x}_i+b))\cdot \frac{\partial }{\partial m}(y_i-(m{x}_i+b))$

这里 $\frac{\partial }{\partial m}(y_i-(m{x}_i+b))=-x_i$，所以：

$\frac{\partial }{\partial m}(y_i-(m{x}_i+b){)}^2=2(y_i-(m{x}_i+b))\cdot (-x_i)=-2x_i(y_i-(m{x}_i+b))$

对复合函数的导数求解上具体来说：

1. 对 $b$ 求导：

• 外部函数 $(y_i-(m{x}_i+b){)}^2$ 导数为 $2(y_i-(m{x}_i+b))$。
• 内部函数 $y_i-(m{x}_i+b)$ 对 $b$ 的导数为 $-1$。

2. 对 $m$ 求导：

• 外部函数 $(y_i-(m{x}_i+b){)}^2$ 导数为 $2(y_i-(m{x}_i+b))$。
• 内部函数 $y_i-(m{x}_i+b)$ 对 $m$ 的导数为 $-x_i$。

最小二乘法的目标函数

最小二乘法的目标是最小化残差平方和（SSE），其形式为：

$\text{SSE}={\sum }_{i=1}^n(y_i-(m{x}_i+b){)}^2$

这个目标函数是针对线性回归问题的。我们试图找到参数 $m$ 和 $b$ 使得预测值 $m{x}_i+b$ 与实际值 $y_i$ 之间的平方误差和最小。

卡尔曼滤波中的目标函数

卡尔曼滤波的目标是递归地估计状态变量，使得估计值与实际值之间的误差最小化。卡尔曼滤波的更新步骤包含两个主要部分：预测和更新。在更新步骤中，我们更新估计的状态和协方差矩阵。

卡尔曼滤波的更新步骤

1. 预测步骤：

• 预测状态： ${\hat{x}}_{k|k-1}=F_k{\hat{x}}_{k-1|k-1}+B_k{u}_k$
• 预测协方差： $P_{k|k-1}=F_k{P}_{k-1|k-1}{F}_k^T+Q_k$

2. 更新步骤：

• 卡尔曼增益： $K_k=P_{k|k-1}{H}_k^T(H_k{P}_{k|k-1}{H}_k^T+R_k{)}^{-1}$
• 更新状态： ${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k(z_k-H_k{\hat{x}}_{k|k-1})$
• 更新协方差： $P_{k|k}=(I-K_k{H}_k)P_{k|k-1}$

残差平方和与卡尔曼滤波的关系

在最小二乘法中，我们最小化的是残差平方和（SSE），即 $\sum (y_i-{\hat{y}}_i{)}^2$。在卡尔曼滤波中，我们最小化的是估计误差协方差矩阵 $P$，这也是一种残差的度量方式。

最小化目标函数的矩阵形式

最小二乘法的目标函数可以用矩阵形式表示。假设 $X$ 是设计矩阵，包含所有输入变量（特征）， $y$ 是目标向量， $\beta$ 是参数向量。目标函数的矩阵形式为：

$J(\beta )=(y-X\beta {)}^T(y-X\beta )$

卡尔曼滤波的目标函数的矩阵形式

卡尔曼滤波的更新步骤实际上是通过最小化预测和实际测量之间的加权误差平方和来进行的。这个加权误差平方和的目标函数是：

$J(\hat{x})=(z_k-H_k\hat{x}{)}^T{R}_k^{-1}(z_k-H_k\hat{x})+(\hat{x}-{\hat{x}}_{k|k-1}{)}^T{P}_{k|k-1}^{-1}(\hat{x}-{\hat{x}}_{k|k-1})$

这里，

• $z_k$ 是测量值，
• $H_k$ 是测量矩阵，
• $R_k$ 是测量噪声协方差，
• ${\hat{x}}_{k|k-1}$ 是预测的状态，
• $P_{k|k-1}$ 是预测的协方差矩阵。
  这个目标函数考虑了测量误差和状态预测误差的加权和。

目标函数与状态更新公式

在卡尔曼滤波中，状态更新的步骤实际上是最小化当前状态估计和新观测值之间的加权平方误差。这个过程可以看作是一个递归的最小二乘估计。具体来看，更新步骤中的公式和目标函数如下：

更新步骤中的公式：
${\hat{\mathbf{x}}}_{k|k}={\hat{\mathbf{x}}}_{k|k-1}+{\mathbf{K}}_k({\mathbf{y}}_k-\mathbf{H}{\hat{\mathbf{x}}}_{k|k-1})$
最小化的目标函数：
${\min }_{{\hat{\mathbf{x}}}_{k|k}}\Vert {\mathbf{y}}_k-\mathbf{H}{\hat{\mathbf{x}}}_{k|k}{\Vert }^2$

各个符号的含义

更新步骤中的公式

1. ${\hat{\mathbf{x}}}_{k|k}$：

• 当前时刻 $k$ 的状态估计（后验估计）。这是我们希望找到的最优估计值。

2. ${\hat{\mathbf{x}}}_{k|k-1}$：

• 当前时刻 $k$ 的状态预测（先验估计）。这是根据之前时刻的状态和系统模型预测出来的。

3. ${\mathbf{K}}_k$：

• 卡尔曼增益。它决定了在预测和观测之间的加权比例，使得估计更准确。

4. ${\mathbf{y}}_k$：

• 当前时刻 $k$ 的观测值。它是通过测量得到的。

5. $\mathbf{H}$：

• 观测矩阵。它描述了状态向量如何映射到观测空间。

6. ${\mathbf{y}}_k-\mathbf{H}{\hat{\mathbf{x}}}_{k|k-1}$：

• 观测残差或创新。它表示观测值和预测值之间的差异。

目标函数

${\min }_{{\hat{\mathbf{x}}}_{k|k}}\Vert {\mathbf{y}}_k-\mathbf{H}{\hat{\mathbf{x}}}_{k|k}{\Vert }^2$
该目标函数中的每个符号与更新步骤中的符号意义相同，解释如下：

1. ${\mathbf{y}}_k$：

• 当前时刻 $k$ 的观测值。

2. $\mathbf{H}{\hat{\mathbf{x}}}_{k|k}$：

• 当前时刻 $k$ 的状态估计在观测空间的映射。

3. $\Vert {\mathbf{y}}_k-\mathbf{H}{\hat{\mathbf{x}}}_{k|k}{\Vert }^2$：

• 观测残差的平方和。它表示观测值和状态估计在观测空间中的差异。

思路说明

1. 预测阶段：

• 我们使用系统模型来预测当前时刻的状态（${\hat{\mathbf{x}}}_{k|k-1}$）和误差协方差（$P_{k|k-1}$）。

2. 更新阶段：

• 计算卡尔曼增益（${\mathbf{K}}_k$），它平衡了预测和观测的不确定性。
• 更新状态估计（${\hat{\mathbf{x}}}_{k|k}$），使其尽可能地靠近实际观测值。
• 更新误差协方差（$P_{k|k}$），使其反映出新的不确定性。

关系

• 残差平方和：在最小二乘法中，我们最小化的是预测值与实际值之间的平方误差和。
• 最小化的目标函数：在卡尔曼滤波中，我们最小化的是测量误差和状态预测误差的加权和，通过更新步骤调整估计的状态和协方差矩阵。
  核心思想都是通过最小化误差（或残差）来优化模型参数，从而得到更好的预测结果。