卡尔曼滤波器（Kalman Filter，KF）算法及仿真

最新推荐文章于 2024-04-20 17:49:48 发布

20岁的挂牌魔法师

最新推荐文章于 2024-04-20 17:49:48 发布

阅读量1.6k

点赞数 1

文章标签：算法传感器 python 机器学习深度学习

本文链接：https://blog.csdn.net/m0_48609216/article/details/113620569

版权

在python下完成卡尔曼滤波的模拟采样与迭代

文章目录

卡尔曼滤波器（Kalman Filter，KF）算法及仿真
1. 贝叶斯滤波器

卡尔曼滤波器（Kalman Filter，KF）算法及仿真

参考链接我的大学老师刘二大人
刘二大人永远的神
github工程如下：点我

提示：以下是本篇文章正文内容，下面案例可供参考

import numpy as np
import matplotlib.pyplot as plt
%matplotlib inline

1. 贝叶斯滤波器

贝叶斯滤波器的算法如下所示：

1：Algorithm Bayes_filter $bel(x_{t-1}), u_t, z_t)$ :
2： for all $x_t$ do
3： $\overline {bel}(x_t) = \int p(x_t \, | \, u_t, x_{t-1})bel(x_{t-1})dx_{t-1}$
4： $bel(x_t)=\eta p ( z_t | x_t) \overline {bel}(x_t)$
5： end for
6： return $bel(x_t)$

贝叶斯滤波的生成模型也被称为隐马尔科夫模型（Hidden Markov Model，HMM）或动态贝叶斯网络（Dynamic Bayes Network， DBN）。

对于离散有限的状态空间，可以用所有可能状态的概率值来表示置信度。但是如果状态空间是无限或连续，则置信度难以表示。

通常有两种置信度的表示方法：

用多元正态分布来表示置信度。对应的滤波算法被称统称为高斯滤波（Gaussian Filter）。
用粒子集来表示置信度。对应的滤波算法被称为粒子滤波（Particle Filter）。

2. 卡尔曼滤波

KF算法用矩来定义置信度，可以对连续状态的置信度进行计算，但是不适用于离散状态空间和混合状态空间。

1：Algorithm Kalman_filter $\mu_{t-1}, \Sigma_{t-1}, u_t, z_t )$ :
2： $\overline \mu_t = A_t \mu_{t-1} + B_t u_t$
3： $\overline \Sigma_t = A_t \Sigma_{t-1}A_t^T + R_t$
4： $K_t = \overline \Sigma_t C_t^T ( C_t \overline \Sigma_t C_t^T + Q_t)^{-1}$
5： $\mu_t = \overline \mu_t + K_t (z_t - C_t \overline \mu_t)$
6： $\Sigma_t = (I - K_t C_t) \overline \Sigma_t$
7： return ${\mu_t, \Sigma_t}$

# 3. Kalman Filter仿真

3.1 仿真模型

3.1.1 问题描述

假设有一个沿直线运动的机器人，每隔一个时间间隔 $\vartriangle t$ ，根据运动 $u_t$ 对自己当前的状态进行预测（Predict），然后利用传感器获得的测量 $z_t$ 对自己的状态进行更新（Update）。

定义机器人的状态 $x_t = \begin{bmatrix}p_t \\ v_t \end{bmatrix}$ ，其中 $p_t$ 表示机器人的位置， $v_t$ 表示机器人的速度。

3.1.2 运动模型

假设机器人可以控制其加速度 $a$ ，则运动迁移方程可以写为：
$KaTeX parse error: No such environment: align at position 7: \begin{̲a̲l̲i̲g̲n̲}̲ p_t &= p_{t-1}…$

把该方程改写为线性方程，可得：
$KaTeX parse error: No such environment: align at position 7: \begin{̲a̲l̲i̲g̲n̲}̲ x_t &= \begin{…$

其中 $\varepsilon_t$ 是控制带来的噪声，假设加速度 $a$ 包含有一个 $\sim N(\mu , \sigma_a^2)$ 的一个噪声。因此该噪声对机器人状态 $x_t$ 带来的噪声可表示为：$ noise_u \sim N(\mu , R_t) $，其中$ \mu = (0, 0)^T $，$ R_t = \begin{bmatrix} ({\frac 12 \triangle t^2})2 & 0 \ 0 & ({\triangle t})^2 \end{bmatrix} \sigma_u^2$。

3.1.3 测量模型

贝叶斯滤波的测量模型并不是使用后验概率 $\, \vert \, z)$ ，而是用似然（likelihood）来进行计算。

假设该仿真模型中的机器人可以获得对当前位置的一个测量量 $z_t$ ，因此可以得到测量模型为：
$KaTeX parse error: No such environment: align at position 7: \begin{̲a̲l̲i̲g̲n̲}̲ z_t &= p_t + \…$

其中 $\delta_t$ 是测量的噪声，该噪声遵从一个均值为0的高斯分布 $\delta_t \sim N( \mu , \sigma_z^2)$ 。

3.2 仿真实验设计

3.2.1 采样函数

在真实的机器人系统中，当我们给定一个控制 $u_t$ ，控制系统将会带有噪声的执行这个控制，在真实物理环境下我们的目的就是去恢复带噪声的执行结果。

在仿真的过程中，我们通过一些采样函数根据控制 $u_t$ 来随机的采样，并将采样的结果作为真实的运动结果，然后就可以观察是否可以通过卡尔曼滤波来消除控制 $u_t$ 所产生的累计噪声。

通常在采样的时候，我们使用正态分布（Normal Distribution）或者三角分布（Triangle Distribution）。

a) 正态分布/高斯分布

正态分布的密度函数为：
$\frac {1} {\sqrt{2 \pi \sigma_2}} e^{- \frac 12 \frac {(x - \mu)^2} {\sigma^2}}$

由于在概率机器人模型中，通常假设分布的均值为 $0$ ，因此均值为 $0$ 的正态分布的密度函数为：
$\frac {1} {\sqrt{2 \pi \sigma_2}} e^{- \frac 12 \frac {x^2} {\sigma^2}}$

3.2.2 运动采样

回忆之前的运动控制方程：
在这里插入图片描述
由图片所示

为了仿真出机器人真实的路径，采用采样的方式来计算真实的机器人状态。在后面根据这个状态来获得观测。

# 控制采样函数，为简化模型我们选用delta t为1
sigma_u = 0.2
def sample_pose(x0, u, sigma):
    A = np.array([1, 1, 0, 1]).reshape((2, 2))
    B = np.array([0.5, 1]).reshape((2, 1))
    noise = np.random.normal(0, sigma)
    x0 = np.array(x0).reshape((2, 1))
    x1 = np.dot(A, x0) + B * u + B * noise
    return x1

sample_pose([0, 0], 1, sigma_u)

3.2.3 测量采样

在这个仿真实验中，机器人只有一个传感器，可以直接获得对机器人位置的测量，因此有下面的测量模型：

在这里插入图片描述

# 测量采样函数
sigma_z = 0.1
def sample_measurement(x, sigma):
    C = np.array([1, 0]).reshape((1, 2))
    x = np.array(x).reshape((2, 1))
    noise = np.random.normal(0, sigma, size=(1, 1))
    z = np.dot(C, x) + noise
    return z

sample_measurement([1, 2], sigma_z)

3.2.4 生成控制序列

在仿真中假设我们共进行10次Kalman Filter迭代，每一次运动的控制 $u_t$ （也就是加速度 $a_t$ ）的值，通过均匀采样得到。

N = 10
U = np.random.uniform(0, 1, size=N)
U

3.2.5 根据控制序列生成无滤波的状态估计

无滤波的状态估计本质上是在假设控制是没有误差的，因此会和真实情况产生巨大的偏移。

假设初始状态 $x_0 = (0, 0)^T$ 。

# 根据上一时刻的状态和当前控制，来估计当前时刻的状态
def caculate_pose(x0, u):
    A = np.array([1, 1, 0, 1]).reshape((2, 2))
    B = np.array([0.5, 1]).reshape((2, 1))
    x0 = np.array(x0).reshape((2, 1))
    x1 = np.dot(A, x0) + B * u
    return x1

x0 = np.array([0, 0]).reshape((1, 2))
X_no_filter = x0

for u in U:
    x = caculate_pose(X_no_filter[-1], u)
    X_no_filter = np.vstack([X_no_filter, np.array(x).reshape((1, 2))])

fig, ax = plt.subplots()
plt.plot(X_no_filter[:, 0], X_no_filter[:,1], label='no-filter')
ax.set_xlabel('Position')
ax.set_ylabel('Velocity')
ax.legend(loc='lower right')

在这里插入图片描述

3.2.6 通过采样获得Ground Truth状态

通过对状态进行采样，来仿真真实情况下，带有噪声的控制所产生的运动结果。

x0 = np.array([0, 0]).reshape((1, 2))
X_ground_truth = x0

for u in U:
    x = sample_pose(X_ground_truth[-1], u, sigma=0.2)
    X_ground_truth = np.vstack([X_ground_truth, np.array(x).reshape((1, 2))])


fig, ax = plt.subplots()  
ax.plot(X_no_filter[:, 0], X_no_filter[:,1], c='b', marker='o', label='no-filter')
ax.plot(X_ground_truth[:,0], X_ground_truth[:,1], c='k', marker='o', label='ground-truth')
ax.set_xlabel('Position')
ax.set_ylabel('Velocity')
ax.legend(loc='lower right')

在这里插入图片描述
从上图可以看出，当控制 $u_t$ 带有噪声的运行时，无滤波的位姿推断与Ground-Truth之间存在着较大的差异，而且随着时间的变化，其差异会越来越大。

可以通过修改的N，来改变状态的数量，以观察这种随机漂移的现象。

3.2.7 根据Ground-Truth状态来生成测量的采样数据

由于传感器观测的是外部物理世界，因此在仿真中，我们根据Ground-Truth的状态来生成带有误差的测量。

但是观测 $z_t$ 并不包含速度的信息，因此在下面的图中，使用Ground-Truth的速度信息来作为观测曲线。因此注意观察红色曲线在水平方向上与Ground-Truth的偏移。

Z = []
for x in X_ground_truth[1:]:
    Z.append(sample_measurement(x, sigma=0.1))

fig, ax = plt.subplots()
ax.plot(X_no_filter[:, 0], X_no_filter[:,1], c='b', marker='o', label='no-filter')
ax.plot(X_ground_truth[:,0], X_ground_truth[:,1], c='k', marker='o', label='ground-truth')
ax.plot(np.squeeze(Z), X_ground_truth[1:,1], c='r', marker='o', label='z')
ax.set_xlabel('Position')
ax.set_ylabel('Velocity')
ax.legend(loc='lower right')

在这里插入图片描述

3.2.8 利用Kalman Filter进行滤波

回忆之前给出的Kalman Filter算法：

1：Algorithm Kalman_filter $\mu, \Sigma_{t-1}, u_t, z_t )$ :
2： $\overline \mu_t = A_t \mu_{t-1} + B_t u_t$
3： $\overline \Sigma_t = A_t \Sigma_{t-1}A_t^T + R_t$
4： $K_t = \overline \Sigma_t C_t^T ( C_t \overline \Sigma_t C_t^T + Q_t)^{-1}$
5： $\mu_t = \overline \mu_t + K_t (z_t - C_t \overline \mu_t)$
6： $\Sigma_t = (I - K_t C_t) \overline \Sigma_t$
7： return ${\mu_t, \Sigma_t}$

def kalman_filter(mu0, Sigma0, u, z, sigma_u=0.2, sigma_z=0.1):
    # 控制预测方程和测量更新方程所需要的系数矩阵
    A = np.array([1, 1, 0, 1]).reshape((2, 2))
    B = np.array([0.5, 1]).reshape((2, 1))
    C = np.array([1, 0]).reshape((1, 2))
    
    # 控制方程和测量方程的误差协方差矩阵
    R = np.array([0.25, 0, 0 , 1]).reshape((2, 2)) * sigma_u ** 2
    Q = np.array([1]).reshape((1,1)) * sigma_z ** 2    
    #Q是干嘛的 - -
    #R是 协方差矩阵 测量方程的
    
    # 对输入的参数进行Tensor维度上的调整，已进行后面的矩阵计算
    mu0 = np.array(mu0).reshape((2, 1))
    Sigma0 = np.array(Sigma0).reshape((2,2))
    z = np.array(z).reshape((1,1))
    
    # 执行控制预测步骤，实现了上面算法的2-3行
    mu1 = np.dot(A, mu0) + B * u
    Sigma1 = np.dot(np.dot(A, Sigma0), A.T) + R
    
    # 进行测量更新步骤，实现了上面算法的4-6行
    Temp = np.linalg.inv(np.dot(np.dot(C, Sigma1), C.T) + Q)
    K = np.dot(np.dot(Sigma1, C.T), Temp)
    mu2 = mu1 + np.dot(K, (z - np.dot(C, mu1)))
    Sigma2 = np.dot((np.eye(2) - np.dot(K, C)), Sigma1)
    
    # 返回Kalman Filter计算出来的新的时刻的均值和方差
    return mu2, Sigma2

对于矩阵 R 与 Q
R：两噪声认为独立，所以是只有对角线上元素非0
在这里插入图片描述
R 与 Q 分为控制步与预测步

x0 = np.array([0, 0]).reshape((1, 2))
X_kalman_filter = x0

sigma0 = np.array([1, 0, 0, 1]).reshape((2,2))
Sigma_kalman_filter = []
Sigma_kalman_filter.append(sigma0)

for u, z in zip(U, Z):
    x, sigma = kalman_filter(X_kalman_filter[-1], Sigma_kalman_filter[-1],
                             u, z, sigma_u=0.4, sigma_z=0.1)
    X_kalman_filter = np.vstack([X_kalman_filter, x.T])
    Sigma_kalman_filter.append(sigma)

fig, ax = plt.subplots()
ax.plot(X_no_filter[:,0], X_no_filter[:,1], c='b', marker='o', label='no-filter')
ax.plot(X_ground_truth[:,0], X_ground_truth[:,1], c='k', marker='o', label='ground-truth')
ax.plot(X_kalman_filter[:,0], X_kalman_filter[:,1], c='r', marker='o', label='kalman-filter')
ax.set_xlabel('Position')
ax.set_ylabel('Velocity')
ax.legend(loc='lower right')

在这里插入图片描述
根据上面结果，可以观察到采用kalman滤波后，能够有效的降低误差。

from matplotlib.patches import Ellipse
# 定义函数getEllipse，根据均值和协方差来返回一个Ellipse对象
def getEllipse(mu, sigma):
    mu = np.squeeze(mu)
    
    scale = np.sqrt(5.991) * 2
    e, v = np.linalg.eig(sigma)
    
    if (e[0] < 0 or e[1] < 0):
        raise ValueError
    
    width = np.sqrt(e[0]) * scale
    length = np.sqrt(e[1]) * scale
    alpha = np.degrees(np.arctan(v[1][0]/v[0][0]))
    ell = Ellipse(xy=mu, width=width, height=length, angle=alpha, 
              alpha=0.2, facecolor='none', edgecolor='r', linestyle = '-',  linewidth=1)
    return ell

ells = [getEllipse(mu, sigma) for mu, sigma in zip(X_kalman_filter, Sigma_kalman_filter)]

fig, ax = plt.subplots()

ax.plot(X_no_filter[:,0], X_no_filter[:,1], 
        c='b', marker='o', label='no-filter')

ax.plot(X_ground_truth[:,0], X_ground_truth[:,1],
        c='k', marker='o', label='ground-truth')

ax.plot(X_kalman_filter[:,0], X_kalman_filter[:,1],
        c='r', marker='o', label='kalman-filter')
ax.set_xlabel('Position')
ax.set_ylabel('Velocity')
ax.legend(loc='lower right')

for ell in ells:
    ax.add_artist(ell)
    
# ax.set_xlim([-10, 25])
# ax.set_ylim([-5, 10])

在这里插入图片描述

从上图的协方差椭圆可以看到，由于观测无关Velocity，因此垂直方向的不确定较大，而Position方向的不确定性较小。随着滤波过程的进行，不确定性逐渐的收敛。

20岁的挂牌魔法师

关注

1
点赞
踩
18

收藏

觉得还不错? 一键收藏
0
评论
卡尔曼滤波器（Kalman Filter，KF）算法及仿真

在python下完成卡尔曼滤波的模拟采样与迭代文章目录卡尔曼滤波器（Kalman Filter，KF）算法及仿真1. 贝叶斯滤波器2. 卡尔曼滤波3.1 仿真模型3.1.1 问题描述3.1.2 运动模型3.1.3 测量模型3.2 仿真实验设计3.2.1 采样函数a) 正态分布/高斯分布3.2.2 运动采样3.2.3 测量采样3.2.4 生成控制序列3.2.5 根据控制序列生成无滤波的状态估计3.2.6 通过采样获得Ground Truth状态3.2.7 根据Ground-Truth状态来生成测量的采样
复制链接

扫一扫