卡尔曼滤波：理论与代码

news2024/9/27 23:30:37

卡尔曼滤波：理论与代码

引言

卡尔曼滤波是一种用于估计系统状态的优化技术，特别适用于含有噪声的测量数据和系统动态变化的情况。本文将简单探讨卡尔曼滤波的理论基础、数学公式的推导，并通过Python代码示例演示其在实际应用中的效果。

一、卡尔曼滤波的基本理论

卡尔曼滤波基于动态系统的模型，通过对系统状态的预测和测量值的融合，提供对系统状态的最优估计。它通过递归地更新估计值，适应系统状态的变化，并考虑测量误差，使得估计更加准确。

二、卡尔曼滤波的基本假设

卡尔曼滤波基于以下两个基本假设：

线性系统： 系统的动态模型和观测模型必须是线性的。
高斯噪声： 系统的过程噪声和测量噪声都应该是高斯分布的。

三、卡尔曼滤波的数学公式推导

在这里插入图片描述

1. 预测步骤

1.1 状态预测：

$\begin{equation}\hat{x}_{k}^- = A \hat{x}_{k-1} + B u_{k-1} \end{equation}$

其中， $\hat{x}_{k}^-$ 是系统状态的先验估计， $A$ 是系统矩阵，$\hat{x}_{k-1}$是上一时刻的估计值， $B$ 是输入矩阵， $u_{k-1}$ 是系统输入。

1.2 协方差预测：

$\begin{equation} P_{k}^- = A P_{k-1} A^T + Q \end{equation}$

其中， $P_{k}^-$ 是状态估计的先验协方差矩阵， $Q$ 是过程噪声协方差。

2. 更新步骤

2.1 卡尔曼增益计算：

$\begin{equation}K_{k} = P_{k}^- H^T (H P_{k}^- H^T + R)^{-1} \end{equation}$

其中， $K_{k}$ 是卡尔曼增益， $H$ 是测量矩阵， $R$ 是测量噪声协方差。

2.2 状态更新：

$\begin{equation}\hat{x}_{k} = \hat{x}_{k}^- + K_{k} (z_{k} - H \hat{x}_{k}^-) \end{equation}$

其中， $\hat{x}_{k}$ 是系统状态的后验估计， $z_{k}$ 是测量值。

2.3 协方差更新：

$\begin{equation}P_{k} = (I - K_{k} H) P_{k}^- \end{equation}$

其中， $P_{k}$ 是状态估计的后验协方差矩阵， $I$ 是单位矩阵。

四、一维卡尔曼滤波的Python代码示例

以下是一个简单的一维卡尔曼滤波的 Python 代码示例。在这个例子中，我们假设系统是一个匀速运动的目标，我们通过测量得到目标的位置信息。

import numpy as np
import matplotlib.pyplot as plt

# 系统参数
A = np.array([[1]])  # 状态转移矩阵
H = np.array([[1]])  # 观测矩阵
B = np.array([[1]])  # 控制输入矩阵
Q = np.array([[0.01]])  # 过程噪声协方差
R = np.array([[0.1]])   # 观测噪声协方差

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

# 模拟数据
true_states = np.array([i for i in range(100)])  # 真实状态，匀速增加
measurements = true_states + np.random.normal(0, np.sqrt(R[0, 0]), 100)  # 加入观测噪声

# 卡尔曼滤波
filtered_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(1) - np.dot(K, H)), P_minus)

    filtered_states.append(x_hat[0])

filtered_states = np.array(filtered_states)

# 可视化结果
plt.figure(figsize=(12, 6))

plt.plot(true_states, label='True States')
plt.plot(measurements, 'ro', label='Measurements')
plt.plot(filtered_states, label='Filtered States')

plt.title('1D Kalman Filtering')
plt.legend()
plt.show()

在这个例子中，模拟了一个一维系统，其中目标的真实状态是一个匀速增加的序列。观测值通过加入观测噪声生成。卡尔曼滤波器通过预测和更新步骤，对目标的状态进行估计。最终结果显示了真实状态、观测值和卡尔曼滤波估计的对比。

五、二维卡尔曼滤波的Python代码示例

下面是一个简单的二维卡尔曼滤波的Python代码示例。这个例子假设系统是一个匀速运动的目标，通过测量得到位置信息。请注意，这个例子中假设噪声是高斯分布的。

import numpy as np
import matplotlib.pyplot as plt

# 系统参数
dt = 1  # 时间步长
A = np.array([[1, dt],
              [0, 1]])

H = np.array([[1, 0],
              [0, 1]])

B = np.array([[0.5 * dt**2],
              [dt]])

Q = np.array([[0.01, 0],
              [0, 0.01]])  # 过程噪声协方差

R = np.array([[0.1, 0],
              [0, 0.1]])   # 测量噪声协方差

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

# 模拟数据
true_states = np.array([[i, 2 * i] for i in range(100)])
measurements = true_states + np.random.multivariate_normal([0, 0], R, 100)  # 加入测量噪声

# 卡尔曼滤波
filtered_states = []

for z in measurements:
    # 预测步骤
    x_hat_minus = np.dot(A, x_hat) + np.dot(B, 1)
    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)

    filtered_states.append(x_hat.flatten())

filtered_states = np.array(filtered_states)

# 可视化结果
plt.figure(figsize=(12, 6))

plt.plot(true_states[:, 0], true_states[:, 1], label='True States')
plt.scatter(measurements[:, 0], measurements[:, 1], color='red', marker='o', label='Measurements')
plt.plot(filtered_states[:, 0], filtered_states[:, 1], label='Filtered States', linestyle='dashed')

plt.title('2D Kalman Filtering')
plt.legend()
plt.show()