【控制算法笔记】卡尔曼滤波（二）——基于状态空间表达的KF基本计算流程以及Python实现

本文是个人学习笔记，包含个人理解，如有错误欢迎指正。

KF算法更多的情况下会用来处理复杂的非线性数据，尤其是当对象特征或检测的状态量不止一个时就得使用状态方程的方法，利用线性代数的计算方式来解决噪声的估计问题。这其中涉及到多个变量数据的融合，协方差的计算等概念。

基于正态分布的样本数据融合-Data Fusion

在实际的数据采集中，通常可能有多个传感器来共同采集一个数据，由于传感器生产运输安装和实际系统的动态扰动，每个传感器采集的数据可能都会存在噪声。人们通常认为这些噪声是符合一个正态分布的随机量，所以我们就可以从多个传感器得到多组符合正态分布的观测样本，怎么将这些数据融合到一起，进而达到降低噪声干扰的目的呢？那就是数据融合（Data Fusion）
假设有两个样本的分布，两个分布都符合正态分布，那从样本数据出发，能够得到两组数据的样本均值和样本方差，一共四个变量。$$E(X)=\frac{1}{n}\sum _{i=0}^n{x}_{i}S(X)=\frac{1}{n-1}\sum _{i=0}^n(x_i-\bar{x}{)}^2$$ 对于我们观测到的两组样本数据，根据卡尔曼估计的原理可以认为在同一个时刻，估计的实际数据满足 $$\hat{Z}=Z_1+K\cdot (Z_2-Z_1)$$ 其中Z一二是k时刻观测到的两个样本数据，当存在一个K能够使得 $\hat{Z}$ 的方差取得最小值的时候，就认为通过k求出的估计值是概率上最符合两个样本的实际值。
根据概率论中基本的方差计算公式可以得到： S ( Z ^ ) = S { Z 1 + K ⋅ ( Z 2 − Z 1 ) } = S { ( 1 − K ) Z 1 + K ⋅ Z 2 } S(\hat{Z}) = S \left \{ Z_1 +K\cdot (Z_2-Z_1) \right \} =S \left \{(1-K)Z_1+K\cdot Z_2 \right \} S(Z^)=S{ Z1​+K⋅(Z2​−Z1​)}=S{ (1−K)Z1​+K⋅Z2​} 由于观测的样本数据之间相互独立，所以可得 $$\Rightarrow S(\hat{Z})=S[(1-K)Z_1]+S(K\cdot Z_2)=(1-K{)}^2\cdot S(Z_1)+K^{2}S(Z_2)$$ 对等式右端求导，并令导数为0可得：$$\Rightarrow S(\hat{Z}{)}^{\prime }=-2\cdot (1-K)\cdot S(Z_1)+2\cdot K\cdot S(Z_2)=0$$ $$\Rightarrow S(\hat{Z}{)}^{\prime }=(1-K)\cdot S(Z_1)+K\cdot S(Z_2)=0$$ $$\Rightarrow k=\frac{S(Z_1)}{S(Z_1)+S(Z_2)}$$ 所以求出了K取值，可以得到概率上最优的融合数据。融合后的数据符合正态分布，且 $$S(\hat{Z})=(1-K{)}^2\cdot S(Z_1)+K^2\cdot S(Z_2)E(\hat{Z})=(1-K)\cdot E(Z_1)+K\cdot E(Z_2)$$

协方差矩阵-Covariance Matrix

当样本数据特征不再是一维数据时，往往将多维的数据计算转化到线性代数的计算，所以为了能在多维数据中计算样本的方差数据，我们必须学习样本的协方差矩阵。
现在假设存在三组样本观测数据：$$X=\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\end{array}\right]Y=\left[\begin{array}{c}{y}_1\\ y_2\\ y_3\end{array}\right]Z=\left[\begin{array}{c}{z}_1\\ z_2\\ z_3\end{array}\right]$$ 每组样本可以计算出各自的方差，总的样本方差可以描述为：$${\sigma }^2=S(X)+S(Y)+S(Z)=\sigma (X{)}^2+\sigma (Y{)}^2+\sigma (Z{)}^2$$ 从而可以将总的样本方差使用二次型表示：$$\vec{S}=\left[\begin{array}{ccc}{{\sigma }_x}^2& {\sigma }_x{\sigma }_y& {\sigma }_x{\sigma }_z\\ {\sigma }_y{\sigma }_x& {{\sigma }_y}^2& {\sigma }_y{\sigma }_z\\ {\sigma }_z{\sigma }_x& {\sigma }_z{\sigma }_y& {{\sigma }_z}^2\end{array}\right]$$ 其中 ${{\sigma }_x}^2$是样本的方差 ${\sigma }_x{\sigma }_y$是x和y的样本协方差。这样的协方差矩阵通过一定的变换可以加快计算机计算的效率。为此需要引入协方差过度矩阵。
我们令协方差过度矩阵为：$$a=\left[\begin{array}{ccc}{x}_1& y_1& z_1\\ x_2& y_2& z_2\\ x_3& y_3& z_3\end{array}\right]-\frac{1}{3}\left[\begin{array}{ccc}1& 1& 1\\ 1& 1& 1\\ 1& 1& 1\end{array}\right]\left[\begin{array}{ccc}{x}_1& y_1& z_1\\ x_2& y_2& z_2\\ x_3& y_3& z_3\end{array}\right]$$ 上式中有第一项矩阵为样本数据矩阵，后一项的值就是样本的平均值。通过过度矩阵相乘可以得到协方差矩阵为：$$P=\frac{1}{3}{a}^{T}a$$ 这样就能通过矩阵的方式加快计算。

状态空间理论

在经典的控制系统中，使用传递函数的形式描述系统输入和输出之间的关系，传递函数所计算的是系统外部的联系。在现代控制系统中，通过对控制系统内部参数建立状态变量进而通过系统微分方程建立状态状态空间表达式。
正是通过状态空间的思想，可以进一步的在此基础上拓展出复杂系统的卡尔曼滤波计算。通常对一个实际的离散系统，可以表达为：$$\begin{array}{c}{X}_k=A{X}_{k-1}+B{u}_{k-1}+w_{k-1}\\ Z_k=H{X}_k+V_k\end{array}$$上式是一个基本的离散形式状态空间表达式，第一行描述输入和状态变量关系，最后一项是系统工作过程中数据产生的过程噪声。第二行描述了输出和状态变量的关系，最后一项是输出数据的测量噪声，在次基础上就可以使用卡尔曼滤波来实时估计当前状态变量的估计值。
通过系统的状态空间方程的形式可以进一步的推导出KF（卡尔曼滤波）的核心参数卡尔曼增益。

状态空间下的KF核心公式

基于上面的离散系统状态空间表达式和数据融合中的概率论计算方法，可以得到，对于
$$\begin{array}{c}{X}_k=A{X}_{k-1}+B{u}_{k-1}+w_{k-1}\\ Z_k=H{X}_k+V_k\end{array}$$ 有预测阶段：$$\begin{array}{c}{\hat{X}}_k^{-}=A{\hat{X}}_{k-1}\\ P_k^{-}=A{P}_{k-1}{A}^T+Q\end{array}$$ 其中X为状态变量，A为状态矩阵，Q为噪声协方差矩阵，P为过程噪声矩阵。
随后在矫正参数阶段有当前时刻的卡尔曼增益：$$K_k=\frac{P_k^{-}{H}^T}{H{P}_k^{-}{H}^T+R}$$
当前时刻的估计值：$${\hat{X}}_k={\hat{X}}_k^{-}+K_k(Z_k-H{\hat{X}}_k^{-})$$ 以及当前时刻的过程估计误差：$$P_k=(I-K_{k}H)P_k^{-}$$

二维KF位置估计-Python

假设对一个移动的小车，使用小车的位置x和y坐标作为状态变量，通过附加位置数据的随机噪声，并迭代来计算估计的位置：

import numpy as np
import matplotlib.pyplot as plt

# 状态空间模型参数
dt = 0.1  # 采样时间间隔
A = np.array([[1, dt],
              [0, 1]])  # 状态转移矩阵，描述系统的演化

H = np.array([[1, 0],
              [0, 1]])  # 观测矩阵，描述测量和状态的关系

# 过程噪声和测量噪声的协方差矩阵
Q = np.diag([0.1, 0.1])  # 过程噪声协方差矩阵
R = np.diag([5, 5])  # 测量噪声协方差矩阵

# 初始状态估计
x_hat = np.array([0, 0])  # 初始状态估计
P = np.diag([4, 1])  # 初始状态协方差矩阵

# 生成实际运动的轨迹数据
true_states = []
for t in range(50):
    x_hat = np.dot(A, x_hat) + np.array([1, 1])  # 在X方向上正向运动，在Y方向上正向运动
    true_states.append(x_hat)

true_states = np.array(true_states)

# 模拟测量数据（带有噪声）
measurements = np.dot(H, true_states.T).T + np.random.multivariate_normal([0, 0], R, size=len(true_states))

# 存储估计值
estimated_states = []

# 卡尔曼滤波过程
for z in measurements:
    # 预测步骤
    x_hat_minus = np.dot(A, x_hat)
    P_minus = np.dot(np.dot(A, P), A.T) + Q

    # 更新步骤
    K = np.dot(np.dot(P_minus, H.T), np.linalg.inv(np.dot(np.dot(H, P_minus), H.T) + R))
    x_hat = x_hat_minus + np.dot(K, z - np.dot(H, x_hat_minus))
    P = np.dot((np.eye(2) - np.dot(K, H)), P_minus)

    estimated_states.append(x_hat)

estimated_states = np.array(estimated_states)

# 绘制真实轨迹和估计值
plt.plot(true_states[:, 0], true_states[:, 1], 'b',label='True States', linestyle='--')
plt.plot(measurements[:, 0], measurements[:, 1], 'ro', label='Measurements')
plt.plot(estimated_states[:, 0], estimated_states[:, 1],'bo', label='Estimated States')

plt.title('State Space Kalman Filter Example')
plt.xlabel('X Position')
plt.ylabel('Y Position')
plt.legend()
plt.show()

将生成的带噪声轨迹数据和KF估计的位置数据用散点的方式绘制出来，其中红色为带噪声的模拟测量位置，蓝色为KF估计的位置。