卡尔曼滤波器实践

最新推荐文章于 2024-05-24 20:05:22 发布
最新推荐文章于 2024-05-24 20:05:22 发布
阅读量2.7k 收藏 20
点赞数 4
分类专栏： 人工智能 文章标签： 人工智能
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/zimiao552147572/article/details/105985280
版权

日萌社

人工智能AI：Keras PyTorch MXNet TensorFlow PaddlePaddle 深度学习实战（不定时更新）

CNN：RCNN、SPPNet、Fast RCNN、Faster RCNN、YOLO V1 V2 V3、SSD、FCN、SegNet、U-Net、DeepLab V1 V2 V3、Mask RCNN

自动驾驶：车道线检测、车速检测、实时通行跟踪、基于视频的车辆跟踪及流量统计

车流量检测实现：多目标追踪、卡尔曼滤波器、匈牙利算法、SORT/DeepSORT、yoloV3、虚拟线圈法、交并比IOU计算

多目标追踪：DBT、DFT、基于Kalman和KM算法的后端优化算法、SORT/DeepSORT、基于多线程的单目标跟踪的多目标跟踪算法KCF

计算交并比IOU、候选框不同表示方式之间的转换

卡尔曼滤波器

卡尔曼滤波器实践

目标估计模型-卡尔曼滤波

匈牙利算法

数据关联：利用匈牙利算法对目标框和检测框进行关联

SORT、DeepSORT

多目标追踪

yoloV3模型

基于yoloV3的目标检测

叉乘：基于虚拟线圈法的车流量统计

视频中的车流量统计

3.4.卡尔曼滤波器实践

学习目标

  • 了解filterpy工具包
  • 知道卡尔曼滤波的实现过程
  • 能够利用卡尔曼滤波器完成小车目标状态的预测

1.filterpy

FilterPy是一个实现了各种滤波器的Python模块，它实现著名的卡尔曼滤波和粒子滤波器。我们可以直接调用该库完成卡尔曼滤波器实现。其中的主要模块包括：

  • filterpy.kalman

    该模块主要实现了各种卡尔曼滤波器，包括常见的线性卡尔曼滤波器，扩展卡尔曼滤波器等。

  • filterpy.common

    该模块主要提供支持实现滤波的各种辅助函数，其中计算噪声矩阵的函数，线性方程离散化的函数等。

  • filterpy.stats

    该模块提供与滤波相关的统计函数，包括多元高斯算法，对数似然算法，PDF及协方差等。

  • filterpy.monte_carlo

    该模块提供了马尔科夫链蒙特卡洛算法，主要用于粒子滤波。

开源代码在：

https://github.com/rlabbe/filterpy/tree/master/filterpy/kalman

我们介绍下卡尔曼滤波器的实现，主要分为预测和更新两个阶段，在进行滤波之前，需要先进行初始化：

  • 初始化

预先设定状态变量dim_x和观测变量维度dim_z、协方差矩阵P、运动形式和观测矩阵H等，一般各个协方差矩阵都会初始化为单位矩阵，根据特定的场景需要相应的设置。


def __init__(self, dim_x, dim_z, dim_u = 0, x = None, P = None,
             Q = None, B = None, F = None, H = None, R = None):
    """Kalman Filter
        Refer to http:/github.com/rlabbe/filterpy

        Method
        -----------------------------------------
         Predict        |        Update
        -----------------------------------------
                        |  K = PH^T(HPH^T + R)^-1
        x = Fx + Bu     |  y = z - Hx
        P = FPF^T + Q   |  x = x + Ky
                        |  P = (1 - KH)P
                        |  S = HPH^T + R
        -----------------------------------------
        note: In update unit, here is a more numerically stable way: P = (I-KH)P(I-KH)' + KRK'

        Parameters
        ----------
        dim_x: int
            dims of state variables, eg:(x,y,vx,vy)->4
        dim_z: int
            dims of observation variables, eg:(x,y)->2
        dim_u: int
            dims of control variables,eg: a->1
            p = p + vt + 0.5at^2
            v = v + at
            =>[p;v] = [1,t;0,1][p;v] + [0.5t^2;t]a
        """

    assert dim_x >= 1, 'dim_x must be 1 or greater'
    assert dim_z >= 1, 'dim_z must be 1 or greater'
    assert dim_u >= 0, 'dim_u must be 0 or greater'

    self.dim_x = dim_x
    self.dim_z = dim_z
    self.dim_u = dim_u

    # initialization
    # predict
    self.x = np.zeros((dim_x, 1)) if x is None else x      # state
    self.P = np.eye(dim_x)  if P is None else P            # uncertainty covariance
    self.Q = np.eye(dim_x)  if Q is None else Q            # process uncertainty for prediction
    self.B = None if B is None else B                      # control transition matrix
    self.F = np.eye(dim_x)  if F is None else F            # state transition matrix

    # update
    self.H = np.zeros((dim_z, dim_x)) if H is None else H  # Measurement function z=Hx
    self.R = np.eye(dim_z)  if R is None else R            # observation uncertainty
    self._alpha_sq = 1.                              # fading memory control
    self.z = np.array([[None] * self.dim_z]).T       # observation
    self.K = np.zeros((dim_x, dim_z))                # kalman gain
    self.y = np.zeros((dim_z, 1))                    # estimation error
    self.S = np.zeros((dim_z, dim_z))                # system uncertainty, S = HPH^T + R
    self.SI = np.zeros((dim_z, dim_z))               # inverse system uncertainty, SI = S^-1

    self.inv = np.linalg.inv
    self._mahalanobis = None                         # Mahalanobis distance of measurement
    self.latest_state = 'init'                       # last process name
  • 预测阶段

接下来进入预测环节，为了保证通用性，引入了遗忘系数α，其作用在于调节对过往信息的依赖程度，α越大对历史信息的依赖越小：

代码如下：


def predict(self, u = None, B = None, F = None, Q = None):
    """
    Predict next state (prior) using the Kalman filter state propagation equations:
                         x = Fx + Bu
                         P = fading_memory*FPF^T + Q

    Parameters
    ----------

    u : ndarray
        Optional control vector. If not `None`, it is multiplied by B
        to create the control input into the system.

    B : ndarray of (dim_x, dim_z), or None
        Optional control transition matrix; a value of None
        will cause the filter to use `self.B`.

    F : ndarray of (dim_x, dim_x), or None
        Optional state transition matrix; a value of None
        will cause the filter to use `self.F`.

    Q : ndarray of (dim_x, dim_x), scalar, or None
        Optional process noise matrix; a value of None will cause the
        filter to use `self.Q`.
    """

    if B is None:
        B = self.B
    if F is None:
        F = self.F
    if Q is None:
        Q = self.Q
    elif np.isscalar(Q):
        Q = np.eye(self.dim_x) * Q

    # x = Fx + Bu
    if B is not None and u is not None:
        self.x = F @ self.x + B @ u
    else:
        self.x = F @ self.x

    # P = fading_memory*FPF' + Q
    self.P = self._alpha_sq * (F @ self.P @ F.T) + Q
    self.latest_state = 'predict'
  • 更新阶段

按下式进行状态的更新：

也可以写为：

其中，y是测量余量，S是测量余量的协方差矩阵。

在实际应用中会做一些微调，使协方差矩阵为：

代码如下：


def update(self, z, R = None, H = None):
    """
    Update Process, add a new measurement (z) to the Kalman filter.
                K = PH^T(HPH^T + R)^-1
                y = z - Hx
                x = x + Ky
                P = (1 - KH)P or P = (I-KH)P(I-KH)' + KRK'

    If z is None, nothing is computed.

    Parameters
    ----------
    z : (dim_z, 1): array_like
        measurement for this update. z can be a scalar if dim_z is 1,
        otherwise it must be convertible to a column vector.

    R : ndarray, scalar, or None
        Optionally provide R to override the measurement noise for this
        one call, otherwise  self.R will be used.

    H : ndarray, or None
        Optionally provide H to override the measurement function for this
        one call, otherwise self.H will be used.
    """

    if z is None:
        self.z = np.array([[None] * self.dim_z]).T
        self.y = np.zeros((self.dim_z, 1))
        return

    z = reshape_z(z, self.dim_z, self.x.ndim)

    if R is None:
        R = self.R
    elif np.isscalar(R):
        R = np.eye(self.dim_z) * R

    if H is None:
        H = self.H

    if self.latest_state == 'predict':
        # common subexpression for speed
        PHT = self.P @ H.T

        # S = HPH' + R
        # project system uncertainty into measurement space
        self.S = H @ PHT + R

        self.SI = self.inv(self.S)


        # K = PH'inv(S)
        # map system uncertainty into kalman gain
        self.K = PHT @ self.SI

        # P = (I-KH)P(I-KH)' + KRK'
        # This is more numerically stable and works for non-optimal K vs
        # the equation P = (I-KH)P usually seen in the literature.
        I_KH = np.eye(self.dim_x) - self.K @ H
        self.P = I_KH @ self.P @ I_KH.T + self.K @ R @ self.K.T


    # y = z - Hx
    # error (residual) between measurement and prediction
    self.y = z - H @ self.x

    self._mahalanobis = math.sqrt(float(self.y.T @ self.SI @ self.y))

    # x = x + Ky
    # predict new x with residual scaled by the kalman gain

    self.x = self.x + self.K @ self.y
    self.latest_state = 'update'

那接下来，我们就是用filterpy中的卡尔曼滤波器方法完成小车位置的预测。

2.小车案例

现在利用卡尔曼滤波对小车的运动状态进行预测。主要流程如下所示：

  • 导入相应的工具包
  • 小车运动数据生成
  • 参数初始化
  • 利用卡尔曼滤波进行小车状态预测
  • 可视化：观察参数的变化与结果

下面我们看下整个流程实现：

  • 导入包
from matplotlib import pyplot as plt
import seaborn as sns
import numpy as np
from filterpy.kalman import KalmanFilter
  • 小车运动数据生成

在这里我们假设小车作速度为1的匀速运动

# 生成1000个位置，从1到1000，是小车的实际位置
z = np.linspace(1,1000,1000) 
# 添加噪声
mu,sigma = 0,1
noise = np.random.normal(mu,sigma,1000)
# 小车位置的观测值
z_nosie = z+noise
  • 参数初始化
# dim_x 状态向量size,在该例中为[p,v]，即位置和速度,size=2
# dim_z 测量向量size，假设小车为匀速，速度为1，测量向量只观测位置，size=1
my_filter = KalmanFilter(dim_x=2, dim_z=1)

# 定义卡尔曼滤波中所需的参数
# x 初始状态为[0,0],即初始位置为0，速度为0.
# 这个初始值不是非常重要，在利用观测值进行更新迭代后会接近于真实值
my_filter.x = np.array([[0.], [0.]])

# p 协方差矩阵，表示状态向量内位置与速度的相关性
# 假设速度与位置没关系，协方差矩阵为[[1,0],[0,1]]
my_filter.P = np.array([[1., 0.], [0., 1.]])

# F 初始的状态转移矩阵，假设为匀速运动模型，可将其设为如下所示
my_filter.F = np.array([[1., 1.], [0., 1.]])

# Q 状态转移协方差矩阵，也就是外界噪声，
# 在该例中假设小车匀速，外界干扰小，所以我们对F非常确定，觉得F一定不会出错，所以Q设的很小
my_filter.Q = np.array([[0.0001, 0.], [0., 0.0001]])

# 观测矩阵 Hx = p
# 利用观测数据对预测进行更新，观测矩阵的左边一项不能设置成0
my_filter.H = np.array([[1, 0]])
# R 测量噪声，方差为1
my_filter.R = 1
  • 卡尔曼滤波进行预测
# 保存卡尔曼滤波过程中的位置和速度
z_new_list = []
v_new_list = []
# 对于每一个观测值，进行一次卡尔曼滤波 
for k in range(len(z_nosie)):
    # 预测过程 
    my_filter.predict()
    # 利用观测值进行更新
    my_filter.update(z_nosie[k])
    # do something with the output
    x = my_filter.x
    # 收集卡尔曼滤波后的速度和位置信息
    z_new_list.append(x[0][0])
    v_new_list.append(x[1][0])

  • 可视化

  • 预测误差的可视化

    # 位移的偏差
dif_list = []
for k in range(len(z)):
    dif_list.append(z_new_list[k]-z[k])
# 速度的偏差
v_dif_list = []
for k in range(len(z)):
    v_dif_list.append(v_new_list[k]-1)
plt.figure(figsize=(20,9))
plt.subplot(1,2,1)
plt.xlim(-50,1050)
plt.ylim(-3.0,3.0)
plt.scatter(range(len(z)),dif_list,color ='b',label = "位置偏差")
plt.scatter(range(len(z)),v_dif_list,color ='y',label = "速度偏差")
plt.legend()

    运行结果如下所示：

2.卡尔曼滤波器参数的变化

首先定义方法将卡尔曼滤波器的参数堆叠成一个矩阵，右下角补0，我们看一下参数的变化。

# 定义一个方法将卡尔曼滤波器的参数堆叠成一个矩阵，右下角补0 
def filter_comb(p, f, q, h, r):
        a = np.hstack((p, f))
        b = np.array([r, 0])
        b = np.vstack([h, b])
        b = np.hstack((q, b))
        a = np.vstack((a, b))
        return a

对参数变化进行可视化：

# 保存卡尔曼滤波过程中的位置和速度
z_new_list = []
v_new_list = []
# 对于每一个观测值，进行一次卡尔曼滤波 
for k in range(1):
    # 预测过程 
    my_filter.predict()
    # 利用观测值进行更新
    my_filter.update(z_nosie[k])
    # do something with the output
    x = my_filter.x
    c = filter_comb(my_filter.P,my_filter.F,my_filter.Q,my_filter.H,my_filter.R)
    plt.figure(figsize=(32,18))
    sns.set(font_scale=4)
    #sns.heatmap(c,square=True,annot=True,xticklabels=False,yticklabels==False,cbar=False)
    sns.heatmap(c,square=True,annot=True,xticklabels=False,yticklabels=False,cbar=False)

对比变换：

从图中可以看出变化的P，其他的参数F，Q，H,R为变换。另外状态变量x和卡尔曼系数K也是变化的。

3.概率密度函数

为了验证卡尔曼滤波的结果优于测量的结果，绘制预测结果误差和测量误差的概率密度函数：

# 生成概率密度图像
z_noise_list_std = np.std(noise)
z_noise_list_avg = np.mean(noise)
z_filterd_list_std = np.std(dif_list)
import seaborn as sns 
plt.figure(figsize=(16,9))
ax = sns.kdeplot(noise,shade=True,color="r",label="std=%.3f"%z_noise_list_std)
ax = sns.kdeplot(dif_list,shade=True,color="g",label="std=%.3f"%z_filterd_list_std)

结果如下：

可以看出卡尔曼滤波器预测的结果误差方差更小，优于测试的结果。

总结：

1.了解filterpy工具包

FilterPy是一个实现了各种滤波器的Python模块，它实现著名的卡尔曼滤波和粒子滤波器。直接调用该库完成卡尔曼滤波器实现。

2.知道卡尔曼滤波的实现过程

卡尔曼滤波器的实现，主要分为预测和更新两个阶段，在进行滤波之前，需要先进行初始化

  • 初始化

预先设定状态变量和观测变量维度、协方差矩阵、运动形式和转换矩阵

  • 预测

对状态变量X和协方差P进行预测

  • 更新

利用观测结果对卡尔曼滤波的结果进行修征

3.能够利用卡尔曼滤波器完成小车目标状态的预测

  • 导入相应的工具包

  • 小车运动数据生成：匀速运动的小车模型

  • 参数初始化：对卡尔曼滤波的参数进行初始化，包括状态变量和观测变量维度、协方差矩阵、运动形式和转换矩阵等

  • 利用卡尔曼滤波进行小车状态预测：使用Filterpy工具包，调用predict和update完成小车状态的预测

  • 可视化：观察参数的变化与结果

    1.预测误差的分布：p,v

    2.参数的变化：参数中变化的是X，P，K，不变的是F，Q，H，R

    1. 误差的概率密度函数：卡尔曼预测的结果优于测量结果

In [1]:

import pandas as pd
from matplotlib import pyplot as plt
import seaborn as sns
import numpy as np
from filterpy.kalman import KalmanFilter

数据生成

In [2]:

# 生成1000个位置，从1到1000，是小车的实际位置
z = np.linspace(1,1000,1000) 
# 添加噪声
mu,sigma = 0,1
noise = np.random.normal(mu,sigma,1000)
# 小车位置的观测值
z_nosie = z+noise

参数初始化

In [8]:

# dim_x 状态向量size,在该例中为[p,v]，即位置和速度,size=2
# dim_z 测量向量size，假设小车为匀速，速度为1，测量向量只观测位置，size=1
my_filter = KalmanFilter(dim_x=2, dim_z=1)

# 定义卡尔曼滤波中所需的参数
# x 初始状态为[0,0],即初始位置为0，速度为0.
# 这个初始值不是非常重要，在利用观测值进行更新迭代后会接近于真实值
my_filter.x = np.array([[0.], [0.]])

# p 协方差矩阵，表示状态向量内位置与速度的相关性
# 假设速度与位置没关系，协方差矩阵为[[1,0],[0,1]]
my_filter.P = np.array([[1., 0.], [0., 1.]])

# F 初始的状态转移矩阵，假设为匀速运动模型，可将其设为如下所示
my_filter.F = np.array([[1., 1.], [0., 1.]])

# Q 状态转移协方差矩阵，也就是外界噪声，
# 在该例中假设小车匀速，外界干扰小，所以我们对F非常确定，觉得F一定不会出错，所以Q设的很小
my_filter.Q = np.array([[0.0001, 0.], [0., 0.0001]])

# 观测矩阵 Hx = p
# 利用观测数据对预测进行更新，观测矩阵的左边一项不能设置成0
my_filter.H = np.array([[1, 0]])
# R 测量噪声，方差为1
my_filter.R = 1

卡尔曼滤波

In [4]:

# 保存卡尔曼滤波过程中的位置和速度
z_new_list = []
v_new_list = []
# 对于每一个观测值，进行一次卡尔曼滤波 
for k in range(len(z_nosie)):
    # 预测过程 
    my_filter.predict()
    # 利用观测值进行更新
    my_filter.update(z_nosie[k])
    # do something with the output
    x = my_filter.x
    # 收集卡尔曼滤波后的速度和位置信息
    z_new_list.append(x[0][0])
    v_new_list.append(x[1][0])

In [6]:

# 位移的偏差
dif_list = []
for k in range(len(z)):
    dif_list.append(z_new_list[k]-z[k])
# 速度的偏差
v_dif_list = []
for k in range(len(z)):
    v_dif_list.append(v_new_list[k]-1)
plt.figure(figsize=(20,9))
plt.subplot(1,2,1)
plt.xlim(-50,1050)
plt.ylim(-3.0,3.0)
plt.scatter(range(len(z)),dif_list,color ='b',label = "位置偏差")
plt.scatter(range(len(z)),v_dif_list,color ='y',label = "速度偏差")
plt.legend()

Out[6]:

<matplotlib.legend.Legend at 0x1281b7b50>

可视化

参数的变换

在卡尔曼滤波的参数中变化的是X，P，K，不变的是F，Q,H，R将卡尔曼滤波器的参数堆叠成一个矩阵，右下角补0，我们看一下参数的变化

In [7]:

# 定义一个方法将卡尔曼滤波器的参数堆叠成一个矩阵，右下角补0 
def filter_comb(p, f, q, h, r):
        a = np.hstack((p, f))
        b = np.array([r, 0])
        b = np.vstack([h, b])
        b = np.hstack((q, b))
        a = np.vstack((a, b))
        return a

In [9]:

c = filter_comb(my_filter.P,my_filter.F,my_filter.Q,my_filter.H,my_filter.R)
plt.figure(figsize=(32,18))
sns.set(font_scale=4)
#sns.heatmap(c,square=True,annot=True,xticklabels=False,yticklabels==False,cbar=False)
sns.heatmap(c,square=True,annot=True,xticklabels=False,yticklabels=False,cbar=False)

Out[9]:

<matplotlib.axes._subplots.AxesSubplot at 0x127db1650>

In [10]:

# 保存卡尔曼滤波过程中的位置和速度
z_new_list = []
v_new_list = []
# 对于每一个观测值，进行一次卡尔曼滤波 
for k in range(1):
    # 预测过程 
    my_filter.predict()
    # 利用观测值进行更新
    my_filter.update(z_nosie[k])
    # do something with the output
    x = my_filter.x
    c = filter_comb(my_filter.P,my_filter.F,my_filter.Q,my_filter.H,my_filter.R)
    plt.figure(figsize=(32,18))
    sns.set(font_scale=4)
    #sns.heatmap(c,square=True,annot=True,xticklabels=False,yticklabels==False,cbar=False)
    sns.heatmap(c,square=True,annot=True,xticklabels=False,yticklabels=False,cbar=False)

概率密度函数

In [11]:

# 生成概率密度图像
z_noise_list_std = np.std(noise)
z_noise_list_avg = np.mean(noise)
z_filterd_list_std = np.std(dif_list)
import seaborn as sns 
plt.figure(figsize=(16,9))
ax = sns.kdeplot(noise,shade=True,color="r",label="std=%.3f"%z_noise_list_std)
ax = sns.kdeplot(dif_list,shade=True,color="g",label="std=%.3f"%z_filterd_list_std)

小车匀速案例

"""
现在利用卡尔曼滤波对小车的运动状态进行预测。主要流程如下所示：
    导入相应的工具包
    小车运动数据生成
    参数初始化
    利用卡尔曼滤波进行小车状态预测
    可视化：观察参数的变化与结果
"""

#导入包
from matplotlib import pyplot as plt
import seaborn as sns
import numpy as np
from filterpy.kalman import KalmanFilter
from pylab import mpl
mpl.rcParams["font.sans-serif"] = ["SimHei"] #支持中文显示
mpl.rcParams["axes.unicode_minus"] = False

#小车运动数据生成
#在这里我们假设小车作速度为1的匀速运动
# 生成1000个位置，从1到1000，是小车的实际位置
z = np.linspace(1,1000,1000)
# 添加噪声
mu,sigma = 0,1
noise = np.random.normal(mu,sigma,1000)
# 小车位置的观测值
z_nosie = z+noise

#参数初始化
# dim_x 状态向量size,在该例中为[p,v]，即位置和速度,size=2
# dim_z 测量向量size，假设小车为匀速，速度为1，测量向量只观测位置，size=1
my_filter = KalmanFilter(dim_x=2, dim_z=1)

# 定义卡尔曼滤波中所需的参数
# x 初始状态为[0,0],即初始位置为0，速度为0.
# 这个初始值不是非常重要，在利用观测值进行更新迭代后会接近于真实值
my_filter.x = np.array([[0.], [0.]])

# p 协方差矩阵，表示状态向量内位置与速度的相关性
# 假设速度与位置没关系，协方差矩阵为[[1,0],[0,1]]
my_filter.P = np.array([[1., 0.], [0., 1.]])

# F 初始的状态转移矩阵，假设为匀速运动模型，可将其设为如下所示
my_filter.F = np.array([[1., 1.], [0., 1.]])

# Q 状态转移协方差矩阵，也就是外界噪声，
# 在该例中假设小车匀速，外界干扰小，所以我们对F非常确定，觉得F一定不会出错，所以Q设的很小
my_filter.Q = np.array([[0.0001, 0.], [0., 0.0001]])

# 观测矩阵 Hx = p
# 利用观测数据对预测进行更新，观测矩阵的左边一项不能设置成0
my_filter.H = np.array([[1, 0]])
# R 测量噪声，方差为1
my_filter.R = 1

#卡尔曼滤波进行预测
# 保存卡尔曼滤波过程中的位置和速度
z_new_list = []
v_new_list = []
# 对于每一个观测值，进行一次卡尔曼滤波
for k in range(len(z_nosie)):
    # 预测过程
    my_filter.predict()
    # 利用观测值进行更新
    my_filter.update(z_nosie[k])
    # do something with the output
    x = my_filter.x
    # 收集卡尔曼滤波后的速度和位置信息
    z_new_list.append(x[0][0])
    v_new_list.append(x[1][0])

#可视化
#预测误差的可视化
# 位移的偏差
dif_list = []
for k in range(len(z)):
    dif_list.append(z_new_list[k]-z[k])
# 速度的偏差
v_dif_list = []
for k in range(len(z)):
    v_dif_list.append(v_new_list[k]-1)

plt.figure(figsize=(20,9))
plt.subplot(1,1,1)
plt.xlim(-50,1050)
plt.ylim(-3.0,3.0)
plt.scatter(range(len(z)),dif_list,color ='b',label = "位置偏差")
plt.scatter(range(len(z)),v_dif_list,color ='y',label = "速度偏差")
plt.legend()
plt.show()

#2.卡尔曼滤波器参数的变化
#首先定义方法将卡尔曼滤波器的参数堆叠成一个矩阵，右下角补0，我们看一下参数的变化。
# 定义一个方法将卡尔曼滤波器的参数堆叠成一个矩阵，右下角补0
def filter_comb(p, f, q, h, r):
        a = np.hstack((p, f))
        b = np.array([r, 0])
        b = np.vstack([h, b])
        b = np.hstack((q, b))
        a = np.vstack((a, b))
        return a

#对参数变化进行可视化：
# 保存卡尔曼滤波过程中的位置和速度
z_new_list = []
v_new_list = []
# 对于每一个观测值，进行一次卡尔曼滤波
for k in range(1):
    # 预测过程
    my_filter.predict()
    # 利用观测值进行更新
    my_filter.update(z_nosie[k])
    # do something with the output
    x = my_filter.x
    c = filter_comb(my_filter.P,my_filter.F,my_filter.Q,my_filter.H,my_filter.R)
    plt.figure(figsize=(32,18))
    sns.set(font_scale=4)
    #sns.heatmap(c,square=True,annot=True,xticklabels=False,yticklabels==False,cbar=False)
    sns.heatmap(c,square=True,annot=True,xticklabels=False,yticklabels=False,cbar=False)

#从图中可以看出变化的P，其他的参数F，Q，H,R为变换。另外状态变量x和卡尔曼系数K也是变化的。
#3.概率密度函数
#为了验证卡尔曼滤波的结果优于测量的结果，绘制预测结果误差和测量误差的概率密度函数：
# 生成概率密度图像
z_noise_list_std = np.std(noise)
z_noise_list_avg = np.mean(noise)
z_filterd_list_std = np.std(dif_list)

import seaborn as sns
plt.figure(figsize=(16,9))
ax = sns.kdeplot(noise,shade=True,color="r",label="std=%.3f"%z_noise_list_std)
ax = sns.kdeplot(dif_list,shade=True,color="g",label="std=%.3f"%z_filterd_list_std)
plt.show()

小车匀加速案例 版本一

"""
现在利用卡尔曼滤波对小车的运动状态进行预测。主要流程如下所示：
    导入相应的工具包
    小车运动数据生成
    参数初始化
    利用卡尔曼滤波进行小车状态预测
    可视化：观察参数的变化与结果
"""

#导入包
from matplotlib import pyplot as plt
import seaborn as sns
import numpy as np
from filterpy.kalman import KalmanFilter
from pylab import mpl
mpl.rcParams["font.sans-serif"] = ["SimHei"] #支持中文显示
mpl.rcParams["axes.unicode_minus"] = False

# u 即 加速度a
u = 2
#小车运动数据生成
#在这里我们假设小车作加速度为2的匀加速运动
# 生成1000个位置，从1到1000，是小车的实际位置
z = np.array([0.5*u*(i**2) for i in range(1000)]) #位移距离公式：1/2 * 加速度a * 单位时间秒的平方。i相当于dt时间。
v = np.array([u*i for i in range(1000)])  #因为加速度a=dv/dt，所以dv=加速度a*dt。i相当于dt时间。
# 添加噪声
mu,sigma = 0,1
noise = np.random.normal(mu,sigma,1000)
# 小车位置的观测值
z_nosie = z+noise


#参数初始化
# dim_x 状态向量size,在该例中为[p,v]，即位置和速度,size=2
# dim_z 测量向量size，假设小车为匀加速，速度为1，测量向量只观测位置，size=1
my_filter = KalmanFilter(dim_x=2, dim_z=1)

# 定义卡尔曼滤波中所需的参数
# x 初始状态为[0,0],即初始位置为0，速度为0.
# 这个初始值不是非常重要，在利用观测值进行更新迭代后会接近于真实值
my_filter.x = np.array([[0.], [0.]])

"""
1.u 即加速度a，亦即状态控制向量。
    加速度a=dv/dt
    m/s^2：米每二次方秒，米除以秒的二次方。
    m*s^-2：米乘以秒的负二次方，负二次方表示二次方的倒数。
2.B 即状态控制矩阵：
    状态控制矩阵的公式为[[1/2 △t^2] [△t]]。那么传入[0.5] 代表 1/2 △t^2 中的系数1/2  ，[1]就是△t中的系数1。
    △t为时间差，此处亦即为遍历位置P的间隔数(步数)作为时间差.
3.初始化位移距离、速度
    #位移距离公式：1/2 * 加速度a * 单位时间秒的平方。下面的i相当于dt时间。
    z = np.array([0.5*u*(i**2) for i in range(1000)])
    #因为加速度a=dv/dt，所以dv=加速度a*dt。下面的i相当于dt时间。
    v = np.array([u*i for i in range(1000)])
4.预测
    predict(u, B = np.array([[0.5],[1]]))
    也可以设置
    my_filter.B = np.array([[0.5],[1]])
    predict(u)
"""
# B 状态控制矩阵：[0.5] 代表 1/2 △t^2 中的系数1/2  ，[1]就是△t中的系数1。△t为时间差，此处亦即为遍历位置P的间隔数(步数)作为时间差
# my_filter.B = np.array([[0.5],[1]])

# p 协方差矩阵，表示状态向量内位置与速度的相关性
# 假设速度与位置没关系，协方差矩阵为[[1,0],[0,1]]
my_filter.P = np.array([[1., 0.], [0., 1.]])

# F 初始的状态转移矩阵，假设为匀加速运动模型，可将其设为如下所示
my_filter.F = np.array([[1., 1.], [0., 1.]])

# Q 状态转移协方差矩阵，也就是外界噪声，
# 在该例中假设小车匀加速，外界干扰小，所以我们对F非常确定，觉得F一定不会出错，所以Q设的很小
my_filter.Q = np.array([[0.0001, 0.], [0., 0.0001]])

# 观测矩阵 Hx = p
# 利用观测数据对预测进行更新，观测矩阵的左边一项不能设置成0
my_filter.H = np.array([[1, 0]])
# R 测量噪声，方差为1
my_filter.R = 1

#卡尔曼滤波进行预测
# 保存卡尔曼滤波过程中的位置和速度
z_new_list = []#位移的预测值
v_new_list = []#速度的预测值
# 对于每一个观测值，进行一次卡尔曼滤波
for k in range(len(z_nosie)):
    # 预测过程
    my_filter.predict(u, B = np.array([[0.5],[1]]))
    # 利用观测值进行更新
    my_filter.update(z_nosie[k])
    # do something with the output
    x = my_filter.x
    # 收集卡尔曼滤波后的速度和位置信息
    z_new_list.append(x[0][0]) #位移的预测值
    v_new_list.append(x[1][0]) #速度的预测值

#可视化
#预测误差的可视化
# 位移的偏差
dif_list = []
#位移/位置(预测值，真实值))
dif_list_pair = []
for k in range(len(z)):
    dif_list.append(z_new_list[k]-z[k]) #位移的预测值 - 位移的真实值 = 位移的偏差
    dif_list_pair.append([z_new_list[k], z[k]]) #位移/位置(预测值，真实值))
# 速度的偏差
v_dif_list = []
#速度(预测值，真实值)
v_dif_list_pair = []
for k in range(len(z)):
    v_dif_list.append(v_new_list[k]-v[k]) #速度的预测值 - 速度的真实值 = 速度的偏差
    v_dif_list_pair.append([v_new_list[k],v[k]]) #速度(预测值，真实值)

print('位移/位置(预测值，真实值)):',dif_list_pair)
print('位移偏差:',dif_list)
print('速度(预测值，真实值):',v_dif_list_pair)
print('速度偏差:',v_dif_list)

plt.figure(figsize=(20,9))
plt.subplot(1,1,1)
plt.xlim(-50,1050)
plt.ylim(-3.0,3.0)
plt.scatter(range(len(z)),dif_list,color ='b',label = "位置偏差")
plt.scatter(range(len(z)),v_dif_list,color ='y',label = "速度偏差")
plt.legend()
plt.show()


plt.figure(figsize=(20, 8))
plt.xlim(-5, 100+5)
plt.ylim(-3, +5)
plt.subplot(3, 2, 1)
plt.scatter(range(len(z)), dif_list, color='b', label="位置偏差")
plt.plot(range(len(z)),[0]*len(z),color='r')
plt.legend()
plt.subplot(3, 2, 2)
plt.scatter(range(len(z)), v_dif_list, color='y', label="速度偏差")
plt.plot(range(len(z)),[0]*len(z),color='r')
plt.legend()
plt.subplot(3, 2, 3)
plt.scatter(range(len(z)), z_nosie, color='b', label="真实位置")
plt.legend()
plt.subplot(3, 2, 4)
plt.scatter(range(len(z)), v, color='y', label="真实速度")
plt.legend()
plt.subplot(3, 2, 5)
plt.scatter(range(len(z)), z_new_list, color='b', label="预测位置")
plt.legend()
plt.subplot(3, 2, 6)
plt.scatter(range(len(z)), v_new_list, color='y', label="预测速度")
plt.legend()
plt.show()

#2.卡尔曼滤波器参数的变化
#首先定义方法将卡尔曼滤波器的参数堆叠成一个矩阵，右下角补0，我们看一下参数的变化。
# 定义一个方法将卡尔曼滤波器的参数堆叠成一个矩阵，右下角补0
def filter_comb(p, f, q, h, r):
        a = np.hstack((p, f))
        b = np.array([r, 0])
        b = np.vstack([h, b])
        b = np.hstack((q, b))
        a = np.vstack((a, b))
        return a

#对参数变化进行可视化：
# 保存卡尔曼滤波过程中的位置和速度
z_new_list = []
v_new_list = []
# 对于每一个观测值，进行一次卡尔曼滤波
for k in range(1):
    # 预测过程
    my_filter.predict()
    # 利用观测值进行更新
    my_filter.update(z_nosie[k])
    # do something with the output
    x = my_filter.x
    c = filter_comb(my_filter.P,my_filter.F,my_filter.Q,my_filter.H,my_filter.R)
    plt.figure(figsize=(32,18))
    sns.set(font_scale=4)
    #sns.heatmap(c,square=True,annot=True,xticklabels=False,yticklabels==False,cbar=False)
    sns.heatmap(c,square=True,annot=True,xticklabels=False,yticklabels=False,cbar=False)

#从图中可以看出变化的P，其他的参数F，Q，H,R为变换。另外状态变量x和卡尔曼系数K也是变化的。
#3.概率密度函数
#为了验证卡尔曼滤波的结果优于测量的结果，绘制预测结果误差和测量误差的概率密度函数：
# 生成概率密度图像
z_noise_list_std = np.std(noise)
z_noise_list_avg = np.mean(noise)
z_filterd_list_std = np.std(dif_list)

import seaborn as sns
plt.figure(figsize=(16,9))
ax = sns.kdeplot(noise,shade=True,color="r",label="std=%.3f"%z_noise_list_std)
ax = sns.kdeplot(dif_list,shade=True,color="g",label="std=%.3f"%z_filterd_list_std)
plt.show()

小车匀加速案例 版本二

import pandas as pd
from matplotlib import pyplot as plt
import seaborn as sns
import numpy as np
from filterpy.kalman import KalmanFilter
# 时间是1000s
t = np.linspace(1,10,1000) 
# 加速度值
a =10;    
# 小车位置
z =1/2*a*(t*t)
# 添加噪声
mu,sigma = 0,1
noise = np.random.normal(mu,sigma,1000)
# 小车位置的观测值
z_nosie = z+noise
# dim_x 状态向量size,在该例中为[p,v，a]，即位置和速度,size=3
# dim_z 测量向量size，假设小车为匀速，速度为1，测量向量只观测位置，size=1
my_filter = KalmanFilter(dim_x=3, dim_z=1)

# 定义卡尔曼滤波中所需的参数
# x 初始状态为[0,0],即初始位置为0，速度为0.
# 这个初始值不是非常重要，在利用观测值进行更新迭代后会接近于真实值
my_filter.x = np.array([[0.], [0.], [0.]])

# p 协方差矩阵
my_filter.P = np.array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])

# F 初始的状态转移矩阵，假设为匀加速运动模型，可将其设为如下所示
my_filter.F = np.array([[1., 1.,0.5], [0., 1.,1.],[0., 0.,1.]])

# Q 状态转移协方差矩阵，也就是外界噪声，
# 在该例中假设小车匀速，外界干扰小，所以我们对F非常确定，觉得F一定不会出错，所以Q设的很小
my_filter.Q = np.array([[0.0001, 0., 0.], [0., 0.0001, 0.], [0., 0., 0.0001]])

# 观测矩阵 Hx = p
# 利用观测数据对预测进行更新，观测矩阵的左边一项不能设置成0
my_filter.H = np.array([[1, 0, 0]])
# R 测量噪声，方差为1
my_filter.R = 1
# 保存卡尔曼滤波过程中的位置
z_new_list = []
# 对于每一个观测值，进行一次卡尔曼滤波 
for k in range(len(z_nosie)):
    # 预测过程 
    my_filter.predict()
    # 利用观测值进行更新
    my_filter.update(z_nosie[k])
    # do something with the output
    x = my_filter.x
    # 收集卡尔曼滤波后的位置信息
    z_new_list.append(x[0][0])
    
# 位移的偏差
dif_list = []
for k in range(len(z)):
    dif_list.append(z_new_list[k]-z[k])
# 生成概率密度图像
z_noise_list_std = np.std(noise)
z_noise_list_avg = np.mean(noise)
z_filterd_list_std = np.std(dif_list)
import seaborn as sns 
plt.figure(figsize=(16,9))
ax = sns.kdeplot(noise,shade=True,color="r",label="std=%.3f"%z_noise_list_std)
ax = sns.kdeplot(dif_list,shade=True,color="g",label="std=%.3f"%z_filterd_list_std)

あずにゃん
关注
  • 4
    点赞
  • 踩
  • 20
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 4
    评论
专栏目录
yolov3实现车流量检测
04-18
更多请加博主 qq 3180433576
卡尔曼滤波的五大公式及python代码示例
热门推荐
hjxu2016的博客
02-25 3万+
一、系统的状态方程 xkx_{k}xk​　是状态分量的 nnn维矢量 AAA 是　n∗nn * nn∗n 的状态转移矩阵，也就是对目标状态转换的猜想模型，是已知的 uk−1u_{k-1}uk−1​ 是新的，让系统可以接受外部控制 BBB 是　n∗cn * cn∗c 矩阵，将输入转换为状态的矩阵 wk−1w_{k-1}wk−1​ 是预测过程的噪声，对应 xkx_{k}xk​中每个分量的噪声，期望为０，协方差为QQQ的高斯白噪声 二、观测方程 HHH　是　m∗nm * nm∗n矩阵，是状态变量（观测
滤波算法与SLAM：Kalman Filter（卡尔曼滤波）算法与示例
最新发布
weixin_48013375的博客
05-24 1049
本章简单介绍Kalman Filter的原理以及运用，附带C++示例具体公式推导不会介绍，具体可参考《视觉SLAM十四讲》以及《概率机器人》相关篇幅本章节内容多参考《概率机器人》中的内容要滤除的是在二维平面的位置信息噪声，假设机器人沿着直线匀速运动那么设状态变量和观测变量为XxyvxvyTXxyvx​vy​TZxyZ=[x,y]Zxy那么显而易见的，状态方程为Xk110Δt0010Δt00100001Xk。
目标跟踪中的卡尔曼滤波kalman_filter
DeepCBW的博客
05-13 2258
目标跟踪中的卡尔曼滤波kalman_filter
Python FilterPy包：贝叶斯滤波器库
snushe♤ 的博客
04-26 1万+
Python FilterPy包：贝叶斯滤波器库安装filterpy示例：kalman filter参考资料 安装filterpy pip install filterpy 注意:运行filterpy包还需要numpy，scipy和matplotlib。 测试filterpy版本： import filterpy filterpy.__version__ #'1.4.5' 常用的模块名： 一、...
python之卡尔曼滤波
weixin_38428439的博客
02-04 969
卡尔曼滤波
python 无迹卡尔曼（filterpy.kalman.UnscentedKalmanFilter）调参
weixin_46180132的博客
05-20 4387
python 无迹卡尔曼（filterpy.kalman）调参 1. filter.kalman.MerweScaledSigmaPoints，Van der Merwe式sigma点选择，UT变换最好用的sigma点选择算法 input： n ：状态维度 alpha ：均值附近sigma点的分布，通常选0.1~1之间。调节中心点与周围点的权重。 beta ：由先验知识，高斯分布选2。 kappa ：k = 3-n output: Wm ：sigma点均值的权重 Wc ：sigma点协方差的权重 可以看到，
卡尔曼滤波器-误差状态卡尔曼滤波器算法实现.zip
03-14
卡尔曼滤波器是一种在信号处理和控制理论领域广泛应用的估计方法，特别是在处理随机噪声和系统不确定性时。误差状态卡尔曼滤波器是卡尔曼滤波器的一种变体，特别适用于处理非线性系统。本文件主要探讨的是如何实现...
自适应卡尔曼滤波器的matlab实现
04-15
自适应卡尔曼滤波器（Adaptive Kalman Filter, AKF）是一种在不确定性和非线性环境下优化数据估计的算法，它扩展了经典的线性卡尔曼滤波理论，使其能够应对系统模型参数随时间变化的情况。MATLAB作为强大的数学计算...
卡尔曼滤波器_MATLAB_卡尔曼滤波器_卡尔曼滤波_
09-30
卡尔曼滤波器是一种在信号处理领域广泛应用的数学算法，特别是在估计理论和控制工程中占有重要地位。MATLAB作为一种强大的数值计算和可视化工具，是实现卡尔曼滤波器的理想平台。本资料集提供了MATLAB程序，帮助用户...
卡尔曼滤波器-基于C语言的扩展卡尔曼滤波器进行姿态解算.zip
03-14
总结来说，这个项目为我们提供了一个学习和实践卡尔曼滤波器，特别是扩展卡尔曼滤波器在姿态解算中应用的机会。通过深入理解和分析C语言实现的代码，我们可以增强在信号处理和控制系统中的实战能力，同时对非线性...
KalmanFilter:卡尔曼过滤器在Python中的基本实现
04-21
卡尔曼滤波 使用python的卡尔曼滤波器的基本实现
CUDAkalmanFilter-master_卡尔曼滤波器_kalman滤波并行_
10-03
《CUDA卡尔曼滤波器：并行计算与实践》 卡尔曼滤波器是一种广泛应用的线性递归估计算法，广泛应用于信号处理、导航、控制理论等领域。在现代计算机科学中，尤其是图形处理和高性能计算中，GPU的并行计算能力使得...
机器学习笔记：卡尔曼滤波
qq_40206371的博客
07-12 4382
插值（interpolation）平滑 （smoothing）注：这一小节的上下标和之后卡尔曼滤波的部分会有一定的差异离散卡尔曼滤波器用于估计离散时间过程的状态变量 （小车的方向，速度等）这个离散时间过程由以下离散随机差分方程描述：定义观测变量 （小车的位置），我们有：记先验和后验估计的误差为 于是先验和后验估计的协方差矩阵为我们接下来推导 测量状态方程的顺序是：如果 先验协方差估计趋近于0——>趋近于0 ——> 上一时刻计算得到的后验估计被作为下一时刻计算的先验估计...
Python 卡尔曼滤波实践
flyfish
03-15 488
Python 卡尔曼滤波实践。从xlsx文件读取的数据是。
卡尔曼滤波应用
终曲
12-11 1333
卡尔曼滤波在目标追踪中的应用
python实现卡尔曼滤波代码详解
leo0308的博客
08-26 1379
卡尔曼滤波代码详解。
目标估计模型-卡尔曼滤波
あずにゃん梓喵的博客
07-20 3478
日萌社 人工智能AI：Keras PyTorch MXNet TensorFlow PaddlePaddle 深度学习实战（不定时更新） 4.4. 目标估计模型-卡尔曼滤波 学习目标 了解卡尔曼滤波器中的状态变量和观测输入 了解目标框的状态更新向量 了解预测边界框的估计 在这里我们主要完成卡尔曼滤波器进行跟踪的相关内容的实现。 初始化：卡尔曼滤波器的状态变量和观测输入 更新状...
卡尔曼滤波原理及代码
m0_63462829的博客
04-25 3488
卡尔曼滤波（Kalman filtering）是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法，它可以在任意含有不确定信息的动态系统中使用，用来对目标的位置进行预测，并且利用预测结果对跟踪的目标进行修正，属于自动控制理论中的一种方法。它很适合持续变化的系统，并且占用内存少，非常适合实时问题与嵌入系统，在单目标还是多目标领域都是很常用的一种算法。例如一个无人驾驶汽车想要在城市中自由行驶，它必须知道自己的确切位置才能进行导航。GPS装置，但缺点是精度可能太低；
使用卡尔曼滤波器的工程问题
06-12
卡尔曼滤波器是一种用于估计系统状态的算法，常用于飞行器导航、控制、目标跟踪等领域。在工程实践中，使用卡尔曼滤波器时需要注意以下问题： 1. 系统模型的建立：卡尔曼滤波器需要一个数学模型来描述系统的动态行为，这个模型需要准确反映实际系统的物理特性，否则会导致估计结果不准确。 2. 噪声的建模：卡尔曼滤波器假设系统存在一定的噪声，需要对噪声进行建模，包括噪声的大小、性质等。如果噪声建模不准确，会影响估计结果的精度。 3. 初始状态的估计：卡尔曼滤波器需要一个初始状态来开始估计过程，如果初始状态不准确，会影响后续估计结果的精度。 4. 参数的选择：卡尔曼滤波器中存在一些参数，如协方差矩阵、观测矩阵等，这些参数的选择会影响估计结果的精度和收敛速度。 5. 实时性的考虑：在一些实时应用中，卡尔曼滤波器需要在实时环境下运行，需要考虑算法的计算复杂度和实时性的要求。 综上所述，使用卡尔曼滤波器需要考虑多个因素，需要根据具体应用场景进行参数选择和模型建立，才能得到准确的估计结果。

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 4
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

あずにゃん

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付：¥1
获取中
扫码支付

您的余额不足，请更换扫码支付或充值

打赏作者

实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值