状态空间模型

从g-h滤波器到卡尔曼滤波

Posted by : zifu on Jan 2, 2024

Category : 数学

Kalman Filter

rlabbe/Kalman-and-Bayesian-Filters-in-Python

现实生活中，我们经常收到外界噪声的干扰。比如在高精度的秤上称量同一件物品时，可能会有不一样的结果，这就是外界噪音的影响，因此我们需要滤波算法，使得系统能够捕捉到我们需要的量，而忽略噪音的存在，这就是各种滤波算法的出发点

g-h 滤波器

g-h滤波器是大量滤波器的基础，卡尔曼滤波器也是 g-h 滤波器的一种形式。其中g是我们用于测量的缩放比例，h是测量值随时间变化的比例。

g越大，估计的结果更趋近于观测值，g越小，估计的结果更趋近于预测值。
h越大，估计信号的波动越大，对于变化速度快的信号可以选择较大的h

g，h参数的trade-off问题是这类滤波器的关键因素，卡尔曼滤波中的g和h是动态变化的。

算法过程

初始化状态x0、增量dx和g、h参数
预测状态x_pred，利用先前的估计值x_est和增量dx
根据measuremen得到的z和预测值x_pred，更新增量dx，并更新估计x_est

最终的滤波结果是所有的估计值

def g_h_filter(data, x0, dx, g, h, dt=1.):
    x_est = x0
    results = []
    for z in data:
        # prediction step
        x_pred = x_est + (dx*dt)
        dx = dx

        # update step
        residual = z - x_pred
        dx = dx + h * (residual) / dt
        x_est = x_pred + g * residual
        results.append(x_est)
    return np.array(results)

ringing 现象是信号处理中很常见的现象，信号以类似正弦的方式波动，高于或低于测量值，随着预测的进行在一段波动后才能找到信号的期望值。h较大或g较大都可能引起ringing现象

离散贝叶斯滤波器

卡尔曼滤波器属于贝叶斯滤波器的一种，贝叶斯滤波也是大部分滤波器的理论基础。首先我们建立一个跟踪一只狗的模型，来描述整个贝叶斯滤波要解决的问题。

问题描述：

在一个长廊中（用一个长度为n的数组表示），在特定的位置有门，我们定义有门的位置为1，没有门的位置为0。有一个检测门的传感器，当检测到门的时候返回1，没有检测到门的时候返回0。我们希望从传感器是否检测到门的信息，来判断该狗的具体位置。为了简化模型，假设狗每一个时间步长只能往右移动一位，当移动到整个数组的最后时，回到数组的最开始。

分析：

这是典型的滤波问题，从带有噪声的观测变量（是否检测到门）来估计隐状态（位置信息）。而贝叶斯滤波器的方法就是根据贝叶斯定理，建立的模型。贝叶斯公式如下，其中P(x)称为先验概率，P(y|x)称为似然函数，P(x|y)为后验概率。P(y)为归一化因子，因为先验概率乘似然函数后不一定是一个概率，要用归一化因子控制使其归一到零到一。

\[P(x|y)=P(x)\frac{P(y|x)}{P(y)}\]

我们针对本问题可以给出如下定义

真实场景：真实的场景信息，这里就是长廊的门位置信息
观测变量\(y_k\)：传感器的检测是否在有门的位置
隐状态\(x_k\)：真正的位置信息（1~n哪个数字）。我们要求解的其实就是\(P(x_k \mid y_k)\)，即根据当前步的观测变量，求解该步的隐变量位置信息的概率。根据贝叶斯公式，\(P(x_k \mid y_k)=P(x_k)\frac{P(y_k \mid x_k)}{P(y_k)}\approx P(x_{k-1})\frac{P(y_k \mid x_{k-1})}{Z_k}\)我们利用上一步的隐状态估计当前隐状态的概率分布。这样不断迭代最终结果收敛。
先验概率prior：\(P(x_{k-1})\)为上一个步计算得到的隐变量的概率分布，即狗出现在每一个位置对应的概率值
似然函数likelihood：即\(P(y_k \mid x_{k-1})\) 为根据上一步的隐变量概率分布，得到当前观测变量的概率分布，即每一个位置是是该传感器检测值的概率。其和不是1，因为每个位置是独立的，不是一个样本。
后验概率posterior：\(P(x_k \mid y_k)\)为根据当前观测变量得到的隐变量的概率分布，即根据当前步的传感器检测是否有门的值，得到当前所在位置的概率分布。

算法过程：

初始化先验概率\(P(x_{0})\)，由于一开始并不知道狗在哪里，所以初始的先验概率就是均匀分布的，即每个点概率值相等。
更新似然函数\(P(y_0, x_0)\)：根据当前的观测值\(y_0\)，计算似然函数\(P(y_0, x_0)\) 方法可以是设置一个scale，当先验概率数组中对应位置的真实数据和当前观测值相等时，则在概先验概率上乘一个scale，代表了似然函数。
更新后验概率\(P(x_1\mid y_0)\)：根据似然函数和先验概率，利用贝叶斯公式预测后验概率。这里要归一化，即求和后该数组除以和，相当于概率公式中的除以\(P(y_k)\)。概率值最高的就是该步骤预测的隐状态，也就是预测位置。
预测下一轮的先验概率\(P(x_1)\)：用更新的后验概率预测下一轮的先验概率，由于有噪声影响，观测值不一定是准确的，因此我们在更新下一轮先验概率的时候不能完全相信这一轮计算的后验概率，在这里定义一个噪声概率分布，和当前后验概率的卷积，因为卷积相当于是原本概率分布对应的点受到卷积核尺寸范围内的加权，这样就不单单依赖于一个点的概率了，而是一个范围，这也是对噪声滤除的核心。
后面每一步都是先预测下一轮的先验概率，然后更新似然函数和后验概率。

随着算法的不断迭代，根据上一轮的先验概率预测本轮的后验概率，然后更新先验概率，由于得到的观测数据越来越多，即使有些观测数据有噪声，最终算法中的后验概率分布中某个位置的概率值与其他位置的概率差距会越来越大，因此随着迭代步骤的增加，跟踪会越来越准。

def discrete_bayes_sim(prior, kernel, measurements, z_prob, hallway):
    posterior = np.array([.1]*10)
    priors, posteriors = [], []
    for i, z in enumerate(measurements):
        prior = predict(posterior, 1, kernel)
        priors.append(prior)

        likelihood = lh_hallway(hallway, z, z_prob)
        posterior = update(likelihood, prior)
        posteriors.append(posterior)
    return priors, posteriors

# change these numbers to alter the simulation
kernel = (.1, .8, .1)
z_prob = 1.0
hallway = np.array([1, 1, 0, 0, 0, 0, 0, 0, 1, 0])

# measurements with no noise
zs = [hallway[i % len(hallway)] for i in range(50)]

priors, posteriors = discrete_bayes_sim(prior, kernel, zs, z_prob, hallway)
interact(animate_discrete_bayes(hallway, priors, posteriors), step=IntSlider(value=1, max=len(zs)*2));

缺点：

由于更新和预测要把整个模型的概率分布全部更新，我们这个问题还只是单变量，如果多个变量的话模型计算量过大，因此更新整个先验后验概率分布并不是一个很好的解决方法。
该滤波器的长度有限，这个问题中我们定义狗移动的一个距离就是数组的一个位置，但是真实世界中由于是连续的，我们要预测其移动了多远，如果用1cm为单位，数组长度会很长，这样也会造成概率分布的计算量过大。

高斯分布

一维高斯分布概率密度公式

\[f(x, \mu, \sigma) = \frac{1}{\sqrt{2\pi\sigma^2}} \exp \Big [{-\frac{1}{2}}{(x-\mu)^2}/\sigma^2 \Big ]\]

高斯分布性质：两个高斯分布相乘还是高斯分布，相加也是高斯分布，其相乘的均值和方差变化如下，两个高斯分布若方差不为零，相乘后方差必减小，分布变尖，均值在两者之间。

\[\begin{aligned}\mu &=\frac{\sigma_1^2\mu_2 + \sigma_2^2\mu_1}{\sigma_1^2+\sigma_2^2}\\ \sigma^2 &=\frac{\sigma_1^2\sigma_2^2}{\sigma_1^2+\sigma_2^2} \end{aligned}\]

大数定理
- 切比雪夫大数定律：设随机变量序列两两相对独立，且期望存在，方差存在且有共同有限上界。\(Y_n = {\frac{1}{n}}{\sum_{i=1}^n}{X_i}\), \(lim_{n->{\infty}}{P[{\mid Y_n-EY_n \mid <{\epsilon}}]=1}\)。将该公式应用于抽样调查，就会有如下结论：随着样本容量n的增加，样本平均数将接近于总体平均数。从而为统计推断中依据样本平均数估计总体平均数提供了理论依据。
- 辛钦大数定律：在切比雪夫大数定律的基础上增加有相同数学期望\({\mu}\)和方差\({\sigma^2}\)的条件，可得\(Y_n\)依概率收敛于\({\mu}\)。
- 伯努利大数定律：频率估计概率的理论依据
中心极限定理
- 同分布中心极限定理：设随机变量序列X独立同分布，则\(Y_n = {\sum_{i=1}^n}{X_i}\), \(EY_n = n{\mu},DY_n=n{\sigma^2}\)，对\(Y_n\) zscore标准化后得\(Y_n^*\)，可以证明当n充分大时，\(Y_n^*\)近似服从正态分布\(N(0,1)\)，则\(Y_n\)服从\(N(n{\mu},n{\sigma^2})\)。由此可见正态分布在概率论中的重要地位。

在贝叶斯滤波器中我们计算了整个模型的先验后验概率，如果能用高斯分布拟合，我们只需要计算其均值和方差，不需要计算其所有每一个点的值，这会大大简化计算。

多维高斯分布:

在一维概率分布中只需要均值和方差就可以描述一个概率分布，但是在高维度中，不仅仅需要每一个维度的均值和方差，还需要不同维度之间的相关性来衡量维度之间的关系。因此引入了协方差和相关系数 \(\begin{aligned}VAR(X) = \sigma_x^2 &= \mathbb E[(X - \mu)^2]\\ COV(X, Y) = \sigma_{xy} &= \mathbb E\big[(X-\mu_x)(Y-\mu_y)\big]\end{aligned}\) 因此使用协方差矩阵来描述多维的正态分布，由于变量1和变量2的协方差与变量2和变量1的协方差结果相同，所以协方差矩阵对称。协方差为0时说明两个变量无关，协方差很大说明两个变量相关性很强。 \(\Sigma = \begin{bmatrix} \sigma_1^2 & \sigma_{12} & \cdots & \sigma_{1n} \\ \sigma_{21} &\sigma_2^2 & \cdots & \sigma_{2n} \\ \vdots & \vdots & \ddots & \vdots \\ \sigma_{n1} & \sigma_{n2} & \cdots & \sigma_n^2 \end{bmatrix}\) 多维正态分布公式如下： \(f(\mathbf{x},\, \mu,\,\Sigma) = \frac{1}{\sqrt{(2\pi)^n|\Sigma|}}\, \exp \Big [{ -\frac{1}{2}(\mathbf{x}-\mu)^\mathsf{T}\Sigma^{-1}(\mathbf{x}-\mu) \Big ]}\) 多维正态分布的每一维度的边缘概率都是正态分布。

相关系数定义如下，在协方差的基础上除以两个变量的方差乘积，类似于归一化，使得相关系数在0到正负1范围内。相关系数为0说明两个随便变量无关，相关系数绝对值为1说明有很强的相关性。 \(\rho_{xy} = \frac{COV(X, Y)}{\sigma_x \sigma_y}\)

两个高维的高斯分布相乘依然是高斯分布，相乘后可以证明其整体分布也是变尖的。 \(\begin{aligned} \mu &= \Sigma_2(\Sigma_1 + \Sigma_2)^{-1}\mu_1 + \Sigma_1(\Sigma_1 + \Sigma_2)^{-1}\mu_2 \\ \Sigma &= \Sigma_1(\Sigma_1+\Sigma_2)^{-1}\Sigma_2 \end{aligned}\)

连续贝叶斯滤波

离散滤波器在真是的连续世界中使用是由局限的，我们不能对其概率分布建模成离散的概率点，因此需要使用连续的概率分布进行建模，而连续的分布中，由中心极限定理证明了高斯分布是最为重要的，因此先使用高斯分布重新对贝叶斯滤波器进行建模。

对比二者关系，我们原先预测prior是使用卷积的方法，相当于用一个窗对其加权，当连续分布的时候，两个分布之间计算就是带窗的，所以不再需要卷积，我们可以直接另两个高斯分布相加。而更新posterior的时候，原先是用scale增大的likelihood和prior点对点相乘，但是当连续分布时候，只需要两个概率分布相乘。由于高斯分布的相加和相乘都还是高斯分布，因此我们只需要利用两个分布的均值和方差进行建模，在由离散转为连续的同时，大大简化了计算量。

\[\begin{array}{l|l|c} \text{discrete Bayes} & \text{Gaussian} & \text{Step}\\ \hline \bar {\mathbf x} = \mathbf x \ast f(\mathbf x) & \bar {x}_\mathcal{N} = x_\mathcal{N} \, \oplus \, f_{x_\mathcal{N}}(\bullet) & \text{Predict} \\ \mathbf x = \|\mathcal L \bar{\mathbf x}\| & x_\mathcal{N} = L \, \otimes \, \bar{x}_\mathcal{N} & \text{Update} \end{array}\]

在离散的时候，我们假设狗每个时间步长移动1，但是在连续的时候，其并不一定移动1，可以假设其速度也满足一个高斯分布，存在一定的波动，因此定了了process_model 。

predict就是process_model的高斯分布和上一轮的posterior满足的高斯分布相加得到prior
updata就是用prior的高斯分布和测量值满足的高斯分布相乘计算新的prior。

process_var = 2.
sensor_var = 4.5
x = gaussian(0., 400.)
process_model = gaussian(1., process_var)
N = 25

dog = DogSimulation(x.mean, process_model.mean, sensor_var, process_var)
zs = [dog.move_and_sense() for _ in range(N)]

xs, priors = np.zeros((N, 2)), np.zeros((N, 2))
for i, z in enumerate(zs):
	# predict 是两个高斯分布相加
    prior = predict(x, process_model)
    # updata是两个高斯分布相乘 
    x = update(prior, gaussian(z, sensor_var))
    priors[i] = prior
    xs[i] = x

一维卡尔曼滤波

一维卡尔曼滤波其实就是用高斯分布建模的连续贝叶斯滤波器，但是引入了卡尔曼增益这个概念，去重新定义整个模型，因此在提出这个滤波器的时候并没有提及贝叶斯滤波，但是从整体的角度重新回看，发现其是他就是一个贝叶斯滤波。由于引入了卡尔曼增益，并用其定义了整个模型，因此重新定义，并可以看到其与连续贝叶斯滤波的关系。

映射关系：（贝叶斯滤波 -> 卡尔曼滤波)

process_model高斯分布的均值\(\mu_{f_x}\) 和方差\(\sigma_{f_x}^2\) -> \(dx, Q\)
预测的先验概率高斯分布的均值\(\bar{\mu}\) 和方差\(\bar{\sigma}^2\) -> \(\bar{x}, \bar P\)
观测变量的高斯分布的均值\(\mu_z\)和方差\(\sigma_z^2\) -> \(z, R\)
定义卡尔曼增益\(K=\frac{\bar P}{\bar P+R}=\frac{\bar{\sigma}^2}{\bar{\sigma}^2+{\sigma_z}^2}\) 其物理意义可以理解为估计占总预测的比例，在0~1之间

预测过程就是两个高斯分布相加，相对简单，但是更新过程是两个高斯分布相乘，计算用新定义的卡尔曼变量推导相乘后的均值和方差：

\[\begin{aligned}\mu &=\frac{\bar\sigma^2\mu_z + \sigma_z^2\bar\mu}{\sigma_z^2+\bar\sigma^2}=K\mu_z+(1-K)\bar\mu=\bar{\mu}+K(\mu_z-\bar\mu)=\bar x+K(z-\bar x)\\ \sigma^2 &=\frac{\sigma_z^2\bar\sigma^2}{\sigma_z^2+\bar\sigma^2}=K\sigma_z^2=KR=(1-K)\bar P \end{aligned}\]

综上所述，原本的预测过程，得到prior，可以改写为如下形式： \(\begin{array}{|l|l|l|} \hline \text{Equation} & \text{Implementation} & \text{Kalman Form}\\ \hline \bar x = x + f_x & \bar\mu = \mu + \mu_{f_x} & \bar x = x + dx\\ & \bar\sigma^2 = \sigma^2 + \sigma_{f_x}^2 & \bar P = P + Q\\ \hline \end{array}\)

原本的更新过程，计算likelihood和posterior可以改写为： \(\begin{array}{|l|l|l|} \hline \text{Equation} & \text{Implementation}& \text{Kalman Form}\\ \hline x = \| \mathcal L\bar x\| & y = z - \bar\mu & y = z - \bar x\\ & K = \frac {\bar\sigma^2} {\bar\sigma^2 + \sigma_z^2} & K = \frac {\bar P}{\bar P+R}\\ & \mu = \bar \mu + Ky & x = \bar x + Ky\\ & \sigma^2 = \frac {\bar\sigma^2 \sigma_z^2} {\bar\sigma^2 + \sigma_z^2} & P = (1-K)\bar P\\ \hline \end{array}\)

算法过程：

初始化滤波器的
预测过程：预测均值\(\bar x\)和方差\(\bar P\)
更新过程：计算卡尔曼增益\(K\) 和残差，并根据残差值更新均值\(x\)，根据卡尔曼增益更新方差\(P\)

之所以写成这种形式是因为当解决高维的卡尔曼滤波问题时，用这种定义可以轻松改写。

多维卡尔曼滤波

多维卡尔曼滤波就是输入参数由一维变为了多维，且满足多维高斯分布，由于多维之间有协方差的约束，因此相对于一维肯定预测效果会更好，不同维度之间会对该维度的预测进行辅助。当设计一个卡尔曼滤波器的时候，大部分是因为存在一个动态模型满足一定的微分方程关系，比如牛顿运动力学中速度，加速度，位移三者的微分关系，这也是卡尔曼滤波在追踪任务中的主要应用。

映射关系：（一维卡尔曼滤波 -> 高维卡尔曼滤波)

process_model高斯分布的均值 \(dx,\)和方差 \(Q\) -> \(\mathbf{F}\)为状态转移矩阵或状态转移函数, \(\mathbf{Q}\)为运动模型的协方差矩阵，也是噪声矩阵， \(\mathbf{B}\)为输入的控制函数或控制矩阵， \(\mathbf{u}\)为系统的输入，控制预测的状态\(\bar{x}\)，这是额外引入的变量。
预测的先验概率高斯分布的均值 \(\bar{x}\)和方差 \(\bar P\) -> \(\bar{\mathbf{x}}, \bar{\mathbf{P}}\) 概念相同，只是变为了高维，因此\(\mathbf{P}\)为协方差矩阵
观测变量的高斯分布的均值\(z\)和方差\(R\) -> \(\mathbf{z}, \mathbf{R}\) 概念相同，只是变为了高维，因此\(\mathbf{R}\)为协方差矩阵
定义测量矩阵\(\mathbf{H}\) 将状态空间映射到测量空间
定义卡尔曼增益\(K\) 衡量估计所占的比值

在预测过程，要设计

\(\mathbf x\), \(\mathbf P\): 状态和其协方差矩阵
\(\mathbf F\), \(\mathbf Q\): 运动模型和噪声协方差矩阵
\(\mathbf{B,u}\): 控制输入和控制函数，这是可选的，也可以没有输入

\(\begin{array}{|l|l|l|} \hline \text{Univariate} & \text{Univariate} & \text{Multivariate}\\ & \text{(Kalman form)} & \\ \hline \bar \mu = \mu + \mu_{f_x} & \bar x = x + dx & \bar{\mathbf x} = \mathbf{Fx} + \mathbf{Bu}\\ \bar\sigma^2 = \sigma_x^2 + \sigma_{f_x}^2 & \bar P = P + Q & \bar{\mathbf P} = \mathbf{FPF}^\mathsf T + \mathbf Q \\ \hline \end{array}\) 在更新过程，要设计

测量函数\(\mathbf{H}\)
测量空间的噪声协方差矩阵\(R\) \(\begin{array}{|l|l|l|} \hline \text{Univariate} & \text{Univariate} & \text{Multivariate}\\ & \text{(Kalman form)} & \\ \hline & y = z - \bar x & \mathbf y = \mathbf z - \mathbf{H\bar x} \\ & K = \frac{\bar P}{\bar P+R}& \mathbf K = \mathbf{\bar{P}H}^\mathsf T (\mathbf{H\bar{P}H}^\mathsf T + \mathbf R)^{-1} \\ \mu=\frac{\bar\sigma^2\, \mu_z + \sigma_z^2 \, \bar\mu} {\bar\sigma^2 + \sigma_z^2} & x = \bar x + Ky & \mathbf x = \bar{\mathbf x} + \mathbf{Ky} \\ \sigma^2 = \frac{\sigma_1^2\sigma_2^2}{\sigma_1^2+\sigma_2^2} & P = (1-K)\bar P & \mathbf P = (\mathbf I - \mathbf{KH})\mathbf{\bar{P}} \\ \hline \end{array}\)

其实就是状态转移矩阵作用后再计算方差，发现其实相当于直接对协方差矩阵左乘状态转移矩阵，并右乘状态转移矩阵的转置。 \(\begin{aligned} \bar{\mathbf P} &= \mathbb E[(\mathbf{Fx - F\bar \mu})(\mathbf{Fx - F\bar\mu})^\mathsf T] = \mathbf F\, \mathbb E[\mathbf{(x- \bar\mu)(x- \bar\mu)}^\mathsf T]\, \mathbf F^\mathsf T=\mathbf F \mathbf P\mathbf F^\mathsf T \end{aligned}\)

这种变换对于更新过程是一样的，测量矩阵\(\mathbf{H}\)矩阵将状态量\(\bar{\mathbf{x}}\)投影都按测量空间，因此其方差投影到测量空间同样左乘测量矩阵，并右乘测量矩阵的转置得\(\mathbf{H\bar PH}^\mathsf T\)。

在一维的时候卡尔曼增益的物理意义可以理解为估计占总预测的比例，在高维依然可以这么理解，高维的逆矩阵不能写成分母，但是可以用分母的形式方便理解，其还是一个比例。 \(\mathbf K = \mathbf{\bar PH}^\mathsf T (\mathbf{H\bar PH}^\mathsf T + \mathbf R)^{-1}\approx \frac{\bar{P}\mathbf H^\mathsf T}{\mathbf{H\bar PH}^\mathsf T + \mathbf R}\)

from scipy.linalg import inv

count = 50
track, zs = compute_dog_data(R_var, Q_var, count)
xs, cov = [], []
for z in zs:
    # predict
    x = F @ x
    P = F @ P @ F.T + Q
    
    #update
    S = H @ P @ H.T + R
    K = P @ H.T @ inv(S)
    y = z - H @ x
    x += K @ y
    P = P - K @ H @ P
    
    xs.append(x)
    cov.append(P)

xs, cov = np.array(xs), np.array(cov)
plot_track(xs[:, 0], track, zs, cov, plot_P=False)

About Zifu Zhang

I am a master of EE in beihang University, interested in deep learning, image compression and AIGC

Email : zifuzhang@buaa.edu.cn

Website : https://bblgbr.github.io