提示：所有代码均已开源，版权所有转载请标注来源，谢谢~
在经过半年时间的断更后，经过无数个flag之后，这次下定决心要在端午假期把这个遗留的问题解决掉。半年时间确实经历了很多人生中重要的事情，详细我还在写~
本期主题是在上篇 “男女声识别” 最后提出的"卡尔曼滤波"学习过程的一个分享，感兴趣的可以自行进一步研究。
声明：本文仅代表个人观点，没有任何技术含量，如果能对您入门卡尔曼滤波有一丢丢的帮助，那我就能烧高香了~
一、背景
某天和我家“狗蛋”开车在一个高速的辅道行驶，这条辅道和高速完全平行，然后手机上的导航突然提醒我靠右行驶进入辅道，这时才发现导航一直认为我俩在高速上行驶，当时以为只是导航卫星精度问题，但事后在回味这个过程中又觉得应该不是简简单单导航精度的问题，因为上学期间看到过这样一句话：炸药威力范围有时候也是需要考虑导航精度问题，既然军事上使用的导航都要考虑精度，那平时老百姓使用的导航卫星定位误差应该更大，那为什么在大部分情况下导航还是如此的精确，最终就引出了本篇文章：“导航的祖师爷-卡尔曼滤波”。
二、结果
老规矩先上结果：
在视频中可以看到，抛出来的橘子在空中划过了一条美丽的弧线（这不是一句废话，后文有延伸），视频中的三个圈分别代表：本次测量结果(红色)、上次结果(绿色)、本次预测结果(蓝色)。可以看到蓝色圈和红色的圈很接近，表示本次的预测结果很接近本次的测量结果(这个倒是一句废话)。它其实表征的不是说预测结果的准确，它是指表征测量结果很准确，预测的结果应该靠近测量结果，这就是卡尔曼滤波的作用(仅限本人理解)。
橘子轨迹校正
ps：从视频中穿的睡衣就能看出来，这篇文章真的脱了很久很久~
三、基础知识
1. 简单例子
我们先通过一个简单的例子来引入卡尔曼滤波
从我们习以为常的“测量”中可以看到：要真实反映测量结果，通常不是量一次就结束，我们会担忧自己量的不准或者测量工具有误差，所以会进行多次测量最终取了“平均”。
把“平均”的过程通过上式转换后，其实就得到了“卡尔曼滤波”的核心：当前的预测值=上次的估计值+增益*(当前测量-上次的估计)，它是根据上次的估计结果和本次的测量结果，最终得到本次我们“认为”的正确结果。
ps：需要强调一点，卡尔曼滤波不是预测，他的目的是校正。可以类比到本文一开始“导航”的例子，卫星的定位是有10m左右的误差(北斗)，而卡尔曼滤波的左右就是来弥补这10m的误差，不是来预测你下一时刻的位置。
2. 理论知识
一系列的推导公式主要是针对上述卡尔曼滤波核心公式中的卡尔曼增益K，怎么表征K以及怎么求取K，就是卡尔曼滤波的灵魂~
在进入正式的卡尔曼滤波的推导前，我们先介绍几个前置知识：
（1） 数据融合
数据融合是要表述可以用“方差”来表征预估值的准确程度。
（2）协方差矩阵
（3）状态空间
“待补充”
（4）完整推导
核心思想是：为了更准确的估计结果，也就是要使后验估计值更接近真实值，所以就是要使两者之间误差的方差最小。
具体推导过程见下：
四、实现
上节最后推导后的卡尔曼滤波计算如下：
我们以“识别橘子轨迹”为例，来实践一下卡尔曼滤波
1. 识别橘子位置
借助cv2的createTrackbar和getTrackbarPos函数，不断调整图像rgb的范围来识别橘子的位置，以此作为每帧数据的测量值；识别代码如下：
# -*- coding: utf-8 -*-
"""
@Author Alien
@Date 2022-12-13
@Describe :查找目标的rgb范围值
"""
import cv2
import numpy as np
def nothing(x):
pass
cv2.namedWindow('image')
cv2.createTrackbar('low_b', 'image', 0, 255, nothing)
cv2.createTrackbar('low_g', 'image', 0, 255, nothing)
cv2.createTrackbar('low_r', 'image', 0, 255, nothing)
cv2.createTrackbar('hig_b', 'image', 0, 255, nothing)
cv2.createTrackbar('hig_g', 'image', 0, 255, nothing)
cv2.createTrackbar('hig_r', 'image', 0, 255, nothing)
frame = cv2.imread(r'C:\Users\Alien\Desktop\orange.jpg')
frame = cv2.resize(frame, (700, 400))
while True:
a = cv2.getTrackbarPos('low_b', 'image')
b = cv2.getTrackbarPos('low_g', 'image')
c = cv2.getTrackbarPos('low_r', 'image')
d = cv2.getTrackbarPos('hig_b', 'image')
e = cv2.getTrackbarPos('hig_g', 'image')
f = cv2.getTrackbarPos('hig_r', 'image')
hsv_img = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
low_orange = np.array([a, b, c])
high_orange = np.array([d, e, f])
mask = cv2.inRange(hsv_img, low_orange, high_orange)
cv2.imshow('image', mask)
k = cv2.waitKey(1) & 0xff
if k == 27:
break
代码运行后将数据分别调整到[0,116,118,54,255,255]时可以将橘子的位置单独标记为白色，其它位置标记为黑色：
ps：调整到[0,7,14,20,101,103]可以得到胸前小熊~
2. 获取每帧橘子坐标位置
借助cv2的findContours和boundingRect来识别橘子坐标，代码如下：
# -*- coding: utf-8 -*-
"""
@Author Alien
@Date 2023/6/24 19:38
@Describe
"""
import cv2
import numpy as np
class OrangeDetector:
def __init__(self, low_orange, high_orange):
self.low_orange = np.array(low_orange)
self.high_orange = np.array(high_orange)
def detect(self, frame):
hsv_img = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
mask = cv2.inRange(hsv_img, self.low_orange, self.high_orange)
contours, _ = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
contours = sorted(contours, key=lambda x: cv2.contourArea(x), reverse=True)
(x, y, w, h) = cv2.boundingRect(contours[0])
box = (x, y, x + w, y + h)
return box
# 按照手动调整，得到橘子的颜色范围
frame = cv2.imread(r'C:\Users\Alien\Desktop\orange.jpg')
low_range = [0, 116, 118]
high_range = [54, 255, 255]
od = OrangeDetector(low_range, high_range)
x, y, x2, y2 = od.detect(frame) # 检测当前帧中橘子的位置
print('橘子的左右边界为[{:},{:}],上下边界为[{:},{:}]'.format(x,x2,y,y2))
3. 卡尔曼滤波实现
实现卡尔曼滤波的五个公式 + 整合每帧橘子测量值的识别 + 随意给定的初始值，可以最终完成对每帧橘子坐标的校正，得到本文一开头的视频。整体代码如下：
# -*- coding: utf-8 -*-
"""
@Author Alien
@Date 2022-12-13
@Describe :以真实橘子轨迹为例子的完整卡尔曼滤波
"""
import cv2
import numpy as np
class OrangeDetector:
def __init__(self, low_orange, high_orange):
self.low_orange = np.array(low_orange)
self.high_orange = np.array(high_orange)
def detect(self, frame):
hsv_img = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
mask = cv2.inRange(hsv_img, self.low_orange, self.high_orange)
contours, _ = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
contours = sorted(contours, key=lambda x: cv2.contourArea(x), reverse=True)
(x, y, w, h) = cv2.boundingRect(contours[0])
box = (x, y, x + w, y + h)
return box
class KalmanFilter:
def __init__(self, DP, MP, p_i_1, x_i_1, H, A, B=0):
# self.kf = cv2.KalmanFilter(DP, CP)
# 测量矩阵H
self.H = np.array(H)
# 状态转移矩阵A
self.A = np.array(A)
# 控制矩阵B
self.B = np.array(B)
# 初始前一次后验误差的协方差矩阵
self.p_after_i_1 = np.array(p_i_1)
# 初始当前后验误差的协方差矩阵
self.p_pre = np.zeros([DP, DP])
# 初始前一次后验估计值
self.x_after_i_1 = np.array(x_i_1)
# 初始当前后验估计值
self.x_pre = np.zeros([DP,1])
# 过程噪声
self.Q = np.identity(DP)
# 测量噪声
self.R = np.identity(MP)*0.1 # 修改测量噪声的大小，因为本次结果的测量值时拿像素估计的，相对过程噪声来说，测量噪声是很小的
# 五连鞭1--先验预测
def cac_pre_prior(self, x_k_1, u_k, A, B):
temp1 = np.dot(A, x_k_1)
temp2 = np.dot(B, u_k)
return temp1 + temp2
# 五连鞭2--先验误差协方差
def cac_pre_p_matrix(self, A, p_k_1, Q):
temp1 = np.dot(A, p_k_1)
temp2 = np.dot(temp1, A.T)
return temp2 + Q
# 五连鞭3--卡尔曼增益
def cac_kalman_gain(self, p_k, H, R):
temp1 = np.dot(p_k, H.T)
temp2 = np.dot(H, temp1) + R
return np.dot(temp1, np.linalg.inv(temp2))
# 五连鞭4--后验预测
def cac_after_prior(self, x_k, z_k, k, H):
temp1 = np.dot(H, x_k)
temp2 = z_k - temp1
temp3 = np.dot(k, temp2)
return x_k + temp3
# 五连鞭5--后验误差协方差
def cac_after_p_matrix(self, k, H, p_K):
temp1 = np.dot(k, H)
temp2 = np.identity(temp1.shape[0]) - temp1
return np.dot(temp2, p_K)
# 预测
def predict1(self, measure_value, u=0):
"""
1、先预测再校正（这个是不合逻辑的，卡尔曼滤波器的原理就是因为我们测量了一个结果，然后这个结果不准确，
所以我们每次的最终目的是通过校正获取当前时刻的准确值，predict1仅仅是逻辑不合理，计算结果是正确的）
2、但是你换个角度去想，先预测就认为初始值代表上次的校准值，先校准就认为初始值代表本次的预测值，
3、其实怎么考虑都是正确的，卡尔曼在乎的是之后不断的校正，就想拿把尺子测量，如果测量次数足够多，
中间随便加几个0的测量结果，也是不会影响最终估计值
@param measure_value:
@param u:
@return:
"""
measure_value = np.array(measure_value).reshape(-1,1)
u = np.array(u)
# 预测(按照前一时刻k-1最优估计值 预测 当前时刻k的估计值)
x_pre = self.cac_pre_prior(self.x_after_i_1, u, self.A, self.B)
p_pre = self.cac_pre_p_matrix(self.A, self.p_after_i_1, self.Q)
k = self.cac_kalman_gain(p_pre, self.H, self.R)
# 校正（按照当前时刻k的估计值 和 当前时刻k的测量值 校正 当前时刻的最优估计）
# （当前时刻的测量值都已经拿到，估计当前时刻有毛用！！！！！！！！！！！！！！）
# （这就是卡尔曼滤波器要解决的问题：因为当前测量的结果不准确啊，你拿把尺子测量一下你就能说你是准确的么，还是要测量多次取平均）
x_after = self.cac_after_prior(x_pre, measure_value, k, self.H)
p_after = self.cac_after_p_matrix(k, self.H, p_pre)
# 替换下一次值
self.x_after_i_1 = x_after
self.p_after_i_1 = p_after
return x_after.reshape(-1).astype('int')
# 预测
def predict(self, measure_value, u=0):
"""
先校正再预测
@param measure_value:
@param u:
@return:
"""
measure_value = np.array(measure_value).reshape(-1, 1)
u = np.array(u)
# 校正（按照当前k-1的估计值 和 当前k-1的测量值 校正 当前k-1的最优估计值）
k = self.cac_kalman_gain(self.p_pre, self.H, self.R)
self.x_after_i_1 = self.cac_after_prior(self.x_pre, measure_value, k, self.H)
self.p_after_i_1 = self.cac_after_p_matrix(k, self.H, self.p_pre)
# 预测（按照当前k-1的最优估计值 预测 下一时刻k的估计值）
self.x_pre = self.cac_pre_prior(self.x_after_i_1, u, self.A, self.B)
self.p_pre = self.cac_pre_p_matrix(self.A, self.p_after_i_1, self.Q)
# 替换下一次值
self.x_after_i_1 = self.x_pre
self.p_after_i_1 = self.p_pre
return self.x_pre.reshape(-1).astype('int')
def put_lable(frame, frame_width, frame_height):
# center = (frame_width-200,20)
center = (20,40)
radius = 10
color_mea = (0,0,255)
color_aft = (255,0,0)
color_pre = (0,255,0)
cv2.circle(frame, center, radius, color_mea, 2, )
cv2.circle(frame, (center[0],center[1]+radius*3), radius, color_aft, 2, )
cv2.circle(frame, (center[0],center[1]+radius*6), radius, color_pre, 2, )
cv2.putText(frame, 'measure', [center[0]+radius*3//2,center[1]+radius*4//5],
cv2.FONT_HERSHEY_SIMPLEX, 1, color_mea, 1)
cv2.putText(frame, 'cur_pret', [center[0]+radius*3//2,center[1]+radius*4//5+radius*3],
cv2.FONT_HERSHEY_SIMPLEX, 1, color_aft, 1)
cv2.putText(frame, 'last_pret', [center[0]+radius*3//2,center[1]+radius*4//5+radius*6],
cv2.FONT_HERSHEY_SIMPLEX, 1, color_pre, 1)
# 解析视频，获取总帧数
cap = cv2.VideoCapture("orange.mp4")
frame_num = int(cap.get(cv2.CAP_PROP_FRAME_COUNT))
# 获取视频宽度
frame_width = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))
# 获取视频高度
frame_height = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
# 初始化保存视频
fourcc = cv2.VideoWriter_fourcc(*"mp4v") # 设置写入视频的格式
fps_video = cap.get(cv2.CAP_PROP_FPS) # 获取视频帧率
videoWriter = cv2.VideoWriter("org_pre.mp4", fourcc, fps_video, (frame_width, frame_height))
# 初始化卡尔曼滤波器
# DP = 4
DP = 2
MP = 2
# H = [[1, 0, 0, 0], [0, 1, 0, 0]]
H = [[1, 0], [0, 1]]
# A = [[1, 0, 1, 0], [0, 1, 0, 1], [0, 0, 1, 0], [0, 0, 0, 1]]
A = [[1, 0], [0, 1]]
x_0 = np.zeros([DP,1])
p_0 = np.zeros([DP, DP])
# 初始化参数
kf = KalmanFilter(DP=DP, MP=MP, p_i_1=p_0, x_i_1=x_0, H=H, A=A)
# 按照手动调整，得到橘子的颜色范围
low_range = [0, 116, 118]
high_range = [54, 255, 255]
od = OrangeDetector(low_range, high_range)
predict_list = [[0,0]]
# %% 预测
for i in range(frame_num):
cap.set(cv2.CAP_PROP_POS_FRAMES,i) # 设置当前处理帧索引
ret, frame = cap.read() # 读当前帧的画面
x, y, x2, y2 = od.detect(frame) # 检测当前帧中橘子的位置
cx = int((x + x2) / 2) # 计算橘子质心位置
cy = int((y + y2) / 2)
# predicted = kf.predict(measure_value=[cx, cy]) # 预测下一次质心位置
predicted = kf.predict1(measure_value=[cx, cy]) # 预测下一次质心位置
cv2.circle(frame, (predict_list[-1][0], predict_list[-1][1]), 20, (0, 255, 0), 4) # 标注上一次校正结果（绿色）
cv2.circle(frame, (cx, cy), 20, (0, 0, 255), 4) # 标注当前测量结果（红色）
cv2.circle(frame, (predicted[0], predicted[1]), 20, (255, 0, 0), 4) # 用当前测量结果和上一次校正结果，得到当前校正结果（蓝色）
predict_list.append(predicted)
# 图上标注
put_lable(frame, frame_width, frame_height)
#显示视频
cv2.imshow("Frame", frame)
# 保存视频
videoWriter.write(frame)
# 设置视频播放中按键的作用
key = cv2.waitKey(100)
if key == 27:
break
videoWriter.release()
4. 参数确定
从上节推导过程中能发现：状态转移矩阵/控制矩阵/测量矩阵都是固定的，初始的协方差矩阵和后验估计值对结果的校正不会有影响，所以重点在过程噪声和测量噪声的确定。由于本实验中测量值是通过像素得到的，相对过程噪声来说，测量噪声是很小的，所以在初始化时将测量噪声的值进行0.1的缩放。（ps：大家可以调整过程噪声和测量噪声的大小，可以更好的理解卡尔曼滤波的作用）
5. 延伸：
按照橘子滑过的弧线，实际上我们是可以拟合出一条曲线，按照曲线就能识别出橘子的落点。类比一下，是不是就可以预测出篮球的落点，再加上篮筐的位置是确定的，进而能判断本次投篮能否入框~
预测结果如下，由于拟合曲线的过程比较简陋，所以得到的曲线很差。通过增加一些拟合条件：比如在出手的瞬间在开始拟合、拟合曲线的两点间隔稍大一些等等，应该都可以增加拟合的效果。
拟合曲线
五、总结
（1）卡尔曼滤波不是预测，它的目的是校正；
（2）如果测量结果的误差比较小就设定测量噪声小一点，让校正的结果更偏向测量值；
六、参考
（1）b站资源： 卡尔曼滤波器-DR_CAN
下期主题：
（1）用python写微信小程序或者写网页；
（2）复习一下神经网络实现“训练小车倒车入库”；
（3）其它–还在寻找
1311
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
