卡尔曼滤波二维实例——跟踪sin正弦函数
目录
前言
之前在嵌入式平台stm32上实现过一维的卡尔曼滤波，可以看下面这篇文章
STM32 Cubemax(十) ——利用一阶卡尔曼滤波处理超声波数据
这次写一个二维跟踪正弦函数的例子，也是用的比较多的情况（卡尔曼滤波的原理啥的就不介绍了）
这次在Python环境下实现，因为python环境下有现成的包可以调，实现起来比较容易。
一、创建卡尔曼滤波器
在python环境下创建卡尔曼滤波器主要有以下几个步骤，1.构建所需维度的对象，2.定义状态转移函数F，3.定义观测矩阵H，4.定义协方差矩阵P，5.定义测量噪声矩阵R，6.定义系统误差矩阵Q，7，赋予初条件
我们方便构建，我们创建一个卡尔曼滤波器的对象
import numpy as np
from filterpy.kalman import KalmanFilter
class KalmanBoxTracker(object):
def __init__(self, point):
pass1.构建所需维度的对象
我们这里去跟踪正弦变换，一共有两个状态输入，4个状态变量（横坐标值，纵坐标值，vx，vy）
# 定义恒速模型，4个状态变量，2个状态输入
self.kf = KalmanFilter(dim_x=4, dim_z=2)2.定义状态转移函数F
我们这里简化为恒速模型，Xk = Xk-1 + Vx * dt，可以得到下面的矩阵
# 状态向量 X = [点的横坐标，点的纵坐标, 点的vx速度，点的vy速度]
# 这里假设是x和y都是匀速运动
self.kf.F = np.array([
[1, 0, 1, 0],
[0, 1, 0, 1],
[0, 0, 1, 0],
[0, 0, 0, 1]
])3.定义观测矩阵H
这里的测量值为我们给定卡尔曼滤波的坐标， Zk = Xk
# 观测矩阵
self.kf.H = np.array([
[1, 0, 0, 0],
[0, 1, 0, 0],
])4.定义协方差矩阵P
这里的协方差矩阵我们一开始对于不可观察的点，认为它的速度无限大
# P是先验估计的协方差，对不可观察的初速度，给予高度不确定性
self.kf.P = np.array([
[10, 0, 0, 0],
[0, 10, 0, 0],
[0, 0, 1000, 0],
[0, 0, 0, 1000]
])5.定义测量噪声矩阵R
测量噪声的参数自己调整，这里我都设为10了
# R是测量噪声的协方差矩阵，即真实值与测量值差的协方差
self.kf.R = np.array([
[10, 0],
[0, 10]
])6.定义系统误差矩阵Q
这里系统误差参数，也是自己调整了，我这里认为系统的误差比较小，所以都设为0.01了
# Q是系统状态变换误差的协方差, 一般认为系统误差很小
self.kf.Q = np.array([
[0.01, 0, 0, 0],
[0, 0.01, 0, 0],
[0, 0, 0.01, 0],
[0, 0, 0, 0.01]
])7，赋予初条件
我们在创建滤波器的时候，将第一个点作为初始值，赋给卡尔曼滤波器，这里用一个变量来存储卡尔曼滤波器的历史状态值。
# Kalman滤波器初始化时，直接用第一次观测结果赋值状态信息
self.kf.x[0:2] = point
# 存储历史时刻的Kalman状态
self.history = []二、更新与预测
我们在为此对象创建两个方法，用来更新卡尔曼滤波器的参数和预测，这里就是直接调用的包里的更新和预测函数，
def update(self, point):
# 调用更新后，清空历史
self.history = []
# 直接调用包里的更新参数
self.kf.update(point)
def predict(self):
# 库自带预测函数
self.kf.predict()
# 更新历史信息
self.history.append(self.kf.x)
return self.history[-1]三、实例跟踪正弦函数
这里我为正弦函数添加了一个高斯噪声，用与给卡尔曼滤波的测量值也添加了一个高斯噪声。
from kalman import KalmanBoxTracker
import numpy as np
import random
import matplotlib.pyplot as plt
import matplotlib
# from pylab import *
# 中文正常显示
matplotlib.rcParams['font.sans-serif'] = ['SimHei']
x = np.arange(-2*np.pi, 2*np.pi, 0.1)
y = []
# y = np.sin(x) + random.gauss(0, 0.5)
for i in range(x.shape[0]):
y.append(np.sin(x[i]) + random.gauss(0,0.1))
pred = []
init_point = np.array([
[x[0], y[0]]
]).reshape(2,1)
kalman = KalmanBoxTracker(init_point)
for i in range(x.shape[0]):
pred.append(kalman.predict())
z_y = y[i] + random.gauss(0, 0.01)
point = np.array([
[x[i], z_y]
]).reshape(2,1)
kalman.update(point)
x_pred = []
y_pred = []
for i in range(x.shape[0]):
x_pred.append(pred[i][0])
y_pred.append(pred[i][1])
plt.figure()
plt.scatter(x,y,c='r', marker='o', label='sinx添加噪声')
plt.scatter(x_pred, y_pred, c='g', marker='*', label='卡尔曼滤波预测')
plt.legend()
plt.show()四、结果
可以看到效果还是不错的，正确的跟踪到了正弦函数，也可以更好里面的参数，看其他的效果。
总结
这个二维的实例，稍加修改就是SORT多目标跟踪里的卡尔曼滤波的实现了。