前言卡尔曼滤波古老而神秘。在数据处理的世界里我们早已拥有诸多优秀的算法低通滤波、高通滤波、半径滤波、K近邻滤波、均值滤波……它们各司其职稳健可靠。然而唯有卡尔曼滤波仿佛是在时间的舞台上挥毫作画——它不仅能够预测未来的形态更让整个系统的可靠性跃升至新的维度。所以我将写下这篇博客让我们走近卡尔曼同时也走进卡尔曼。主要参考以下文章https://blog.csdn.net/Felaim/article/details/109111285?spm1001.2014.3001.5506https://blog.csdn.net/Felaim/article/details/109111285?spm1001.2014.3001.5506https://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/https://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/正文一、卡尔曼滤波其实在实际控制系统中我们常常面临一个棘手的问题如何融合多个传感器的数据因为我们通常不会轻易地完全相信某一个传感器的输出。例如激光雷达在遇到反光或透光物体时采集到的数据会变得不可靠——这时我们多么希望能借助相机数据来补偿雷达的不足。再比如开车导航时在开阔的室外我们信赖GPS但一旦进入隧道GPS信号减弱甚至丢失此时就只能更多依赖手机的陀螺仪IMU。而本文重点讨论的视觉跟踪也面临类似困境当跟踪的目标被遮挡时我们无法直接知道下一秒目标会去哪里。这时就需要利用之前的数据对下一时刻做出预测直到遮挡结束再更新测量值。正是这些场景让卡尔曼滤波有了用武之地。1、简介卡尔曼滤波是一种递归的最优估计算法核心是“预测—更新”先根据系统运动规律猜一下下一时刻的状态再拿实际传感器数据去修正这个猜测权重由两者各自的不确定度决定。它诞生于1960年由卡尔曼提出克服了传统滤波需要存储全部历史数据且无法处理非平稳系统的缺点很快就在阿波罗导航等任务中落地。后来为了应对现实中的非线性问题又发展出扩展卡尔曼滤波、无迹卡尔曼滤波以及自适应、鲁棒等变体甚至与深度学习结合。如今它的身影无处不在自动驾驶中融合GPS、激光雷达和相机手机导航里用IMU补偿隧道中的信号丢失机器人定位、视觉跟踪的目标遮挡预测乃至金融时序分析都离不开这个“在时间舞台上作画”的算法。2、核心公式其实说到这个算法其实也还是很简单具体就是下面这五个核心的公式。关于这五个公式的数学推导我也会在本文章中体现出来。3、公式数学推导实际例子直接套用上面那些矩阵公式理解起来还是有些困难——矩阵毕竟太抽象。所以我们先从一个一维的例子入手。假设有一辆车以1 m/s的速度做直线运动车上装有GPS可以测量它行驶的距离。理论上经过时间 Ts秒车子应该走 Ts米。但在实际情况中由于轮子可能打滑实际位移未必等于 Ts米同时GPS信号也会上下波动。我们就用这个例子来推导卡尔曼滤波五个核心公式在一维情况下的具体形式。前提知识在开始推导之前我们需要对小学知识做一个回忆。一组数据的和除以数据的个数就是平均数它能反映一组数据的平均水平。每个数据与平均数之差的平方和的平均值就是方差。方差反映的是一组数据的离散程度或波动大小——方差越大数据越分散方差越小数据越集中。特殊地方差为0时所有数据都等于平均数。在实际生活中绝大多数数据都服从或近似服从正态分布。正态分布曲线关于 xμ平均值对称曲线的“胖瘦”由标准差 σ即方差的平方根决定。数据落在 [μ−σ,μσ] 这个区间内的概率约为68%落在区间外的概率约32%而落在更宽的区间如 [μ−2σ,μ2σ]]内的概率则更高约95%。总之数据落在平均值附近区间内的概率明显大于落在远离平均值的区间外的概率。开始计算假如我们通过 1 m/s 的速度乘以时间 dt 来预测小车的位置记预测值为 x1并假设它的方差为 σ1²1 m即标准差 σ11 m这个数值可以随便取随着卡尔曼滤波的进行会逐渐收敛到合理值。那么在 dt 时间后预测位置 x1 的均值是 1×dt而它的可能取值大约在 1×dt±1 的范围内因为 ±1倍标准差包含了约 68% 的概率。换句话说x1大致会在 1×dt1 和1×dt−1 之间波动。同时我们假设GPS测量的移动距离也有4m的波动既σ2²4 m即标准差 σ22 m这个数值一般传感器厂家会给的。那么我们就可以得到下表然而上述推导隐含了一个假设小车的运动完全精确地遵循“1 m/s 的匀速模型”。实际上路面摩擦、微小加减速、风阻等随机扰动会使真实速度发生波动导致模型预测值与真实轨迹之间出现额外的偏差。这种未建模的随机加速度称为过程噪声。为了反映这种不确定性我们需要在每一步预测中增加一个过程噪声方差 Q。于是预测方差不再保持固定值而是更新为σ² σ1²Q其中 σ1² 是上一时刻的最优估计方差 σ²就是新时刻的 σ1²。Q 越大表明模型越不可靠滤波器会更加信任测量值。引入 Q 后估计方差不会无限减小而是稳定在一个合理的平衡值从而更真实地描述动态系统。下面是引入Q后的计算表格好了通过上面的实际例子我们现在对卡尔曼滤波在一维的应用应该是有了一个初步的概念实际上:状态小车位置 x标量控制输入速度 u1m/s作用时间 dt状态转移矩阵F1位置直接传递控制矩阵Bdt观测矩阵H1直接测量位置过程噪声方差Q2测量噪声方差R4m²在k时刻对应上面那5个公式就是:其实这就很好的解释了卡尔曼滤波的过程下面就让我们来扩展为多维矩阵形式。推导扩展a、前提已知条件那么经过上面的案例实际上我们对卡尔曼滤波的运算也就清楚了。现在就让我再在上面例子的基础上新增一些东西比如小车的位置为p小车的速度为v小车外部加速度为aGPS测量位置和上面一样方差为4。好的现在开始计算的话就是这里 Pk其实和上面一维的方差道理一样的不同的是我们这里这个叫做协方差矩阵关于协方差我在下面补充处说明如果不需要可以跳过。补充协方差矩阵用于描述多个随机变量之间的方差与相关性。对角线元素每个变量自身的方差如位置方差、速度方差。非对角线元素两个变量之间的协方差表示它们的变化是否同步正负表示相关方向。在卡尔曼滤波中状态向量如位置和速度的协方差矩阵记为P它表示状态估计的不确定性及变量间耦合关系。end补充b、预测步公式骤推导那么我们可以有0时刻 位置为P0 速度为V0dt时刻 位置为P0V0dt 速度继续为V0 (这里我们认为dt很小 忽略了a的影响)如果加入a的控制的话那么dt时刻 位置为P0V0dt1/2adt² 速度为 V0adt这就是第一个公式的来源。所以就有了c、更新步骤推导更新步骤的核心是将卡尔曼滤波的预测结果与传感器实测数据进行最优融合同时迭代更新协方差矩阵以降低状态估计的不确定性。以上便是一维卡尔曼滤波在多维情形下的自然扩展。借助一维例子中的直观理解预测与测量的加权融合可以更好地把握多维滤波的核心思想。整个卡尔曼滤波过程本质上就是不断重复“预测”与“更新”两个步骤的递推循环。二、实际应用1、应用场景开发一个实时人脸跟踪系统利用摄像头采集视频流结合卡尔曼滤波算法对人脸位置进行预测和跟踪。系统能够持续输出预测框蓝色和检测框绿色并通过预测框“追击”检测框的方式直观展示滤波效果。2、关键技术a. Haar 级联分类器人脸检测原理基于 Viola-Jones 框架使用 Haar-like 特征和 AdaBoost 训练得到的级联分类器。作用在灰度图像中快速检测人脸区域返回边界框(x, y, w, h)。说明:这里人脸识别的方式不是关键关键是卡尔曼滤波所以也可以用yolo等方法。b. 卡尔曼滤波Kalman Filter—— 核心目标对人脸中心点(x, y)及其速度(vx, vy)进行最优估计即使检测偶尔失败也能平滑预测位置。3、代码实现import cv2 import numpy as np import os import urllib.request # 1. 自定义卡尔曼滤波器 class KalmanFilter: 二维匀速运动模型的卡尔曼滤波器 状态向量 x [x, y, vx, vy]^T - x, y : 目标中心坐标 - vx, vy: 速度分量 def __init__(self, dt1.0): self.dt dt # ---------- 状态转移矩阵 F ---------- # 根据匀速运动模型位置 上一位置 速度 * dt速度保持不变 # [ x_new ] [1, 0, dt, 0] [ x ] # [ y_new ] [0, 1, 0, dt] [ y ] # [vx_new] [0, 0, 1, 0] [vx] # [vy_new] [0, 0, 0, 1] [vy] self.F np.array([ [1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1] ], dtypenp.float32) # ---------- 观测矩阵 H ---------- # 我们只能直接观测到位置 (x, y)不能直接观测速度 # 所以观测值 z H * xH 将状态空间映射到观测空间 self.H np.array([ [1, 0, 0, 0], [0, 1, 0, 0] ], dtypenp.float32) # ---------- 过程噪声协方差矩阵 Q ---------- # 描述系统模型的不确定性如速度突变、未建模的加速度 # 这里简化为对角矩阵数值越小表示模型越可信 self.Q np.eye(4, dtypenp.float32) * 0.01 # ---------- 观测噪声协方差矩阵 R ---------- # 描述检测器人脸检测的噪声数值越大表示检测结果越不可信 self.R np.eye(2, dtypenp.float32) * 0.1 # ---------- 误差协方差矩阵 P ---------- # 初始值较大表示初始状态非常不确定随着滤波进行会收敛 self.P np.eye(4, dtypenp.float32) * 100.0 # ---------- 状态向量 x ---------- # 初始化为 0第一次检测后会通过 set_state 或 correct 赋值 self.x np.zeros((4, 1), dtypenp.float32) def predict(self): 预测步骤卡尔曼滤波公式 1 和 2 -------------------------------------------------- 1. 状态预测: x_pred F * x 2. 协方差预测: P_pred F * P * F^T Q 返回预测的位置 (x, y) # 公式 (1) 状态预测 self.x self.F self.x # 公式 (2) 协方差预测 self.P self.F self.P self.F.T self.Q # 返回预测的位置二维 return self.x[:2].flatten() def correct(self, measurement): 更新步骤卡尔曼滤波公式 3、4、5 -------------------------------------------------- 3. 卡尔曼增益: K P_pred * H^T * (H * P_pred * H^T R)^{-1} 4. 状态更新: x_opt x_pred K * (z - H * x_pred) 5. 协方差更新: P_opt (I - K * H) * P_pred 参数 measurement: 检测到的目标中心 (x, y) 返回更新后的位置 (x, y) # 将测量值转为列向量 z np.array(measurement, dtypenp.float32).reshape(2, 1) # 新息残差: 测量值 - 预测的观测值 y z - self.H self.x # 新息协方差: S H * P_pred * H^T R S self.H self.P self.H.T self.R # 公式 (3) 卡尔曼增益 K self.P self.H.T np.linalg.inv(S) # 公式 (4) 状态更新融合测量值 self.x self.x K y # 公式 (5) 协方差更新标准形式要求数值稳定可改用 Joseph 形式 self.P (np.eye(4) - K self.H) self.P return self.x[:2].flatten() def set_state(self, x, y, vx0, vy0): 手动设置状态向量通常用于初始化 self.x np.array([[x], [y], [vx], [vy]], dtypenp.float32) # 辅助函数下载 Haar 级联文件 def download_haarcascade(filenamehaarcascade_frontalface_default.xml): 如果当前目录下不存在指定的级联文件则从 OpenCV GitHub 下载并提示用户 if os.path.exists(filename): return filename url fhttps://raw.githubusercontent.com/opencv/opencv/master/data/haarcascades/{filename} print(f未找到人脸检测模型文件 {filename}正在从网络下载...) try: urllib.request.urlretrieve(url, filename) print(f下载完成已保存为 {filename}) except Exception as e: raise IOError(f下载失败{e}\n请手动下载 {filename} 并放在当前目录下。) return filename # 2. 人脸跟踪器同时返回预测框和检测框 class KalmanFaceTracker: def __init__(self, cascade_filenamehaarcascade_frontalface_default.xml, detection_interval1): :param cascade_filename: 级联分类器文件名默认在当前目录查找 :param detection_interval: 检测间隔1 每帧检测 # 确保模型文件存在自动下载 cascade_path download_haarcascade(cascade_filename) self.face_cascade cv2.CascadeClassifier(cascade_path) if self.face_cascade.empty(): raise IOError(f无法加载分类器: {cascade_path}) self.kf KalmanFilter(dt1.0) self.detection_interval detection_interval self.frame_count 0 self.last_known_size (100, 100) # 默认人脸尺寸用于绘制预测框 self.is_initialized False # 是否已获得第一次检测 def detect_face(self, gray_frame): 在灰度图中检测最大人脸返回中心点和边界框 faces self.face_cascade.detectMultiScale( gray_frame, scaleFactor1.1, minNeighbors5, minSize(30, 30) ) if len(faces) 0: return None, None # 取面积最大的人脸通常是最靠近摄像头的人 x, y, w, h max(faces, keylambda f: f[2] * f[3]) center (x w // 2, y h // 2) return center, (x, y, w, h) def update(self, frame): 处理一帧图像 返回: - pred_bbox: 预测框 (x, y, w, h) (蓝色) - det_bbox: 检测框 (x, y, w, h) (绿色如果没有检测则为 None) gray cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) self.frame_count 1 # ---------- 1. 卡尔曼预测每帧执行 ---------- pred_center self.kf.predict() pred_x, pred_y int(pred_center[0]), int(pred_center[1]) # 用上一次已知的人脸尺寸生成预测框保证预测框大小稳定 w_pred, h_pred self.last_known_size pred_bbox (pred_x - w_pred // 2, pred_y - h_pred // 2, w_pred, h_pred) # ---------- 2. 按间隔进行人脸检测 ---------- det_bbox None if self.frame_count % self.detection_interval 0: center, bbox self.detect_face(gray) if center is not None: det_bbox bbox # 更新已知人脸尺寸为下一帧的预测框提供参考 self.last_known_size (bbox[2], bbox[3]) # 卡尔曼更新用检测值修正滤波器 self.kf.correct(center) if not self.is_initialized: self.is_initialized True return pred_bbox, det_bbox def reset(self): 重置跟踪器状态用于重新开始跟踪 self.kf.x np.zeros((4, 1), dtypenp.float32) self.kf.P np.eye(4, dtypenp.float32) * 100.0 self.frame_count 0 self.is_initialized False self.last_known_size (100, 100) # 3. 主程序 def main(): cap cv2.VideoCapture(0) # 检测间隔设为 1每帧都检测响应最快也可改为 2~5 以提高帧率 tracker KalmanFaceTracker(detection_interval1) print(按 q 退出按 r 重置跟踪器) print(蓝色框: 卡尔曼预测框会追击绿色检测框) print(绿色框: 实际人脸检测框观测值) while True: ret, frame cap.read() if not ret: break pred_bbox, det_bbox tracker.update(frame) # 绘制预测框蓝色 px, py, pw, ph pred_bbox cv2.rectangle(frame, (px, py), (px pw, py ph), (255, 0, 0), 2) cv2.putText(frame, Prediction, (px, py - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 0), 1) # 绘制检测框绿色 if det_bbox is not None: dx, dy, dw, dh det_bbox cv2.rectangle(frame, (dx, dy), (dx dw, dy dh), (0, 255, 0), 2) cv2.putText(frame, Detection, (dx, dy - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1) # 绘制预测中心到检测中心的连线黄色直观显示“追击”效果 pred_center (px pw // 2, py ph // 2) det_center (dx dw // 2, dy dh // 2) cv2.line(frame, pred_center, det_center, (0, 255, 255), 2) # 图例说明 cv2.putText(frame, Blue: Predicted Green: Detected, (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 1) cv2.imshow(Face Tracking - Prediction chasing Detection, frame) key cv2.waitKey(1) 0xFF if key ord(q): break elif key ord(r): tracker.reset() print(跟踪器已重置) cap.release() cv2.destroyAllWindows() if __name__ __main__: main()上面是python代码需要python和opencv的开发环境并且需要有usb或者电脑摄像头才能运行。4、运行效果结语数学是一门精妙而优雅的语言。生在这个时代我们是幸运的——数学知识空前丰沛AI工具俯拾皆是获取真知的渠道从未如此宽广。只要我们沉心静气步步为营每一道曾经高不可攀的技术门槛终将成为我们拾级而上、攀向下一座高峰的坚实阶梯。
