卡尔曼滤波实战：让OpenCV目标追踪稳如磁吸

1. 这不是“高大上”的数学游戏而是让摄像头真正“盯住”移动物体的底层逻辑你有没有试过用树莓派加个普通USB摄像头做目标追踪一开始挺兴奋装好OpenCV跑通YOLOv5检测框小球一出现框就跟着动——看起来像那么回事。可只要目标稍微加速、被遮挡半秒、或者摄像头轻微抖动框就开始“飘”甚至直接跟丢。我第一次在实验室调试云台时看着舵机疯狂左右摆动却始终追不上一个匀速走直线的纸杯心里直犯嘀咕检测模型明明很准为什么“跟踪”这一步总像喝醉了后来才发现问题根本不在模型精度而在于我们把“检测”和“追踪”当成两个割裂的环节来处理。检测帧与帧之间是独立的快照它告诉你“此刻目标在哪”但不回答“下一帧它大概会去哪”。而卡尔曼滤波就是专门干这个事的——它不靠猜也不靠堆算力而是用一套极其精巧的数学框架在“传感器测量值”和“系统运动模型”之间不断做加权融合动态生成对目标位置、速度、加速度最可信的估计。它不是魔法是概率论和线性代数在现实世界里的落地实践把“我知道的”测量和“我推断的”模型预测揉在一起给出比任何单一来源都更稳、更准、更抗干扰的答案。这个标题里“卡尔曼滤波”是方法论“目标追踪”是应用场景二者缺一不可。脱离具体追踪任务谈卡尔曼容易陷入纯公式推导的泥潭只讲追踪不深挖滤波原理又容易变成调包调参的流水线工人。我写这篇就是想带你从一块STM32开发板、一个OpenCV窗口、两个舵机开始亲手把这套逻辑跑通。你会看到当卡尔曼滤波真正嵌入到你的追踪 pipeline 里那个原本“飘忽不定”的检测框会变得像被磁铁吸住一样稳。它解决的不是理论问题而是你调试云台时凌晨三点还在抓狂的实操痛点——延迟、抖动、遮挡恢复慢。适合所有正在做智能硬件、机器人视觉、无人机跟随或工业质检的朋友无论你是刚学完矩阵乘法的大学生还是写了十年嵌入式的老工程师只要你需要让机器“看得准、跟得稳”这篇就是为你写的。2. 为什么非得是卡尔曼——目标追踪中的三大“拦路虎”与它的破局逻辑2.1 目标追踪的三个真实世界困境先说清楚我们面对的从来不是教科书里光滑无噪的轨迹。真实场景中目标追踪要同时扛住三座大山第一座传感器噪声。摄像头拍出来的坐标从来不是“绝对真实”的。CMOS传感器有读出噪声光照变化带来亮度波动镜头畸变让边缘像素位置偏移甚至USB传输带宽不足都会导致图像丢帧或微小位移。我拿同一块标定板在固定光照下连续采集100帧用OpenCV的cv2.findCirclesGrid提取角点X坐标标准差居然达到±1.8像素——这还只是静态标定目标一动噪声叠加运动模糊单帧检测框的中心坐标抖动常常超过3~5像素。如果直接把每一帧的检测结果喂给舵机云台就会像帕金森患者一样高频震颤。第二座模型不确定性。我们总希望目标按某种规律运动匀速、匀加速、圆周……但现实是人走路会突然停顿、小车转弯有侧滑、无人机受阵风影响会偏航。单纯用上一帧速度预测下一帧位置即“恒速模型”在目标急停时预测点会惯性冲出去老远导致跟踪框“飞”到目标前方而在目标突然加速时预测又严重滞后。我在STM32上实现过纯预测PID控制的云台结果就是目标匀速时跟得挺好一拐弯就甩脱再回头找时已经丢失。第三座数据关联与遮挡。当画面中出现多个相似目标比如一群穿同样校服的学生或者目标被短暂遮挡手伸过来、另一物体经过检测模型可能输出多个框甚至漏检。这时候光靠单帧坐标无法判断哪个框属于“原目标”。传统方法靠IOU交并比匹配但IOU在遮挡后恢复时极易误配——新出现的框和旧预测位置IOU低系统就认为“目标已消失”转而跟踪另一个物体。这正是多目标追踪MOT里“ID Switch”问题的根源。2.2 卡尔曼滤波如何系统性拆解这三座山卡尔曼滤波不是万能膏药它的强大在于提供了一套闭环反馈、概率加权、动态更新的框架恰好对应上述三个痛点对抗传感器噪声它不信任任何一次测量。每次收到新检测坐标zₖ不是直接采纳而是计算一个“卡尔曼增益”Kₖ这个增益本质上是一个权重系数它衡量“这次测量有多可信” vs “我的预测模型有多靠谱”。如果测量噪声大比如弱光下检测框模糊Kₖ就自动变小更多相信预测如果测量很准强光下清晰目标Kₖ变大快速向测量值靠拢。这个自适应过程让输出状态x̂ₖ天然具备平滑性。容纳模型不确定性它把目标运动建模为一个“状态向量”比如[x, y, vₓ, v_y]位置速度。预测步Predict用状态转移矩阵F乘以上一时刻最优估计x̂ₖ₋₁得到先验预测x̂ₖ⁻同时用过程噪声协方差矩阵Q量化“模型本身有多不准”——Q越大表示你越不确定目标会不会突然加速/转向滤波器就越“保守”不会盲目相信预测给测量留出更大修正空间。这比硬编码一个“最大加速度”阈值灵活得多。支撑数据关联与遮挡恢复卡尔曼本身是单目标设计但它输出的“预测位置不确定性椭圆由Pₖ协方差矩阵决定”是关联的黄金依据。当检测框出现时计算它与各跟踪器预测位置的马氏距离Mahalanobis Distance而非简单欧氏距离。马氏距离考虑了预测的不确定性——如果预测位置很“松散”Pₖ大即使检测框离得稍远也可能被接受反之若预测极精准Pₖ小只有非常近的框才被匹配。遮挡期间滤波器持续用预测步推进状态Pₖ随时间增大不确定性扩散一旦目标重现只要检测框落入这个扩大的不确定性椭圆内就能瞬间“认出”并重置Pₖ实现无缝恢复。这比单纯计数“丢失几帧”可靠得多。提示很多人以为卡尔曼滤波必须配合复杂运动模型。其实对于大多数云台追踪场景一个简单的二维恒速模型CV Model就足够了。它的状态向量仅4维[x, y, vₓ, v_y]F矩阵是[[1,0,Δt,0], [0,1,0,Δt], [0,0,1,0], [0,0,0,1]]Q矩阵可设为diag([σₓ², σ_y², σ_vx², σ_vy²])。参数调优的关键不是追求理论最优而是让Q的尺度与你实际目标的加速度范围匹配——我测过室内小车最大加速度约0.5 m/s²对应Δt0.1s时σ_vx取0.05就非常稳。2.3 为什么不是其他滤波器——卡尔曼的不可替代性市面上还有粒子滤波PF、UKF无迹卡尔曼、甚至深度学习跟踪器如ByteTrack。它们各有优势但在嵌入式实时追踪场景卡尔曼仍是首选粒子滤波PF能处理强非线性、非高斯噪声但需要成百上千个粒子STM32跑不动树莓派也吃力。它用计算换精度而云台需要的是毫秒级响应。UKF比EKF扩展卡尔曼更准避免雅可比矩阵求导但计算量仍是标准卡尔曼的3倍以上。对于CV模型这种线性度很高的场景UKF带来的精度提升微乎其微却显著增加CPU负担。深度学习跟踪器如ByteTrack依赖YOLO等检测器本身计算开销巨大且需要大量标注数据训练。它解决的是“检测关联”端到端问题但底层关联逻辑依然常嵌入卡尔曼或其变种如SORT算法就用标准卡尔曼做状态估计。纯学习方法在小样本、新场景泛化性差而卡尔曼的物理模型是通用的。所以当你看到“基于STM32与OpenCV的多模式舵机云台目标追踪”这类项目时背后真正的技术脊梁十有八九是卡尔曼滤波。它不是最炫的但它是在资源受限、实时性要求高、物理模型清晰这三重约束下最平衡、最可靠、最容易落地的选择。理解它你就拿到了打开智能视觉追踪大门的那把基础钥匙。3. 从数学符号到代码变量卡尔曼滤波核心五步的逐行拆解3.1 状态向量与建模先想清楚“我要跟踪什么”一切始于定义状态向量x。这不是随便选的它必须包含你关心的所有动态量并能通过观测摄像头坐标直接或间接推导出来。对于二维平面内的目标追踪最常用的是恒速模型Constant Velocity, CVx [x_position, y_position, x_velocity, y_velocity]^T即4维向量。为什么选这个因为绝大多数室内移动目标小车、人、无人机在短时间尺度0.5秒内速度变化相对缓慢加速度可视为噪声。它比纯位置模型2维多了速度信息能预测下一帧大概位置又比恒加速模型6维简单避免过度拟合噪声。状态转移矩阵F描述“如果没有外部干扰状态如何自然演化”。假设帧间隔为Δt例如OpenCV处理一帧耗时33ms则Δt0.033则F [[1, 0, Δt, 0], [0, 1, 0, Δt], [0, 0, 1, 0], [0, 0, 0, 1]]解释新位置 旧位置 旧速度 × Δt新速度 旧速度假设无加速度。这是一个线性关系所以标准卡尔曼滤波完全适用。注意F矩阵必须与你的Δt严格对应。如果你在OpenCV里用cv2.getTickCount()测得实际帧率是25fpsΔt0.04s就不能用30fpsΔt≈0.033的F。我吃过亏——用固定Δt算F结果云台在高帧率时超调低帧率时滞后。解决方案是每帧动态计算Δt并实时更新F。STM32上可用SysTick定时器树莓派用time.time()。3.2 过程噪声Q给模型“留余地”的艺术Q矩阵代表你对运动模型不确定性的量化。它不是凭空设定的而是基于你对目标物理行为的理解。Q通常设为对角阵每个对角元对应状态分量的过程噪声方差。对于CV模型Q主要影响速度分量的“漂移”程度。经验公式Q [[(Δt^3)/3, 0, (Δt^2)/2, 0], [0, (Δt^3)/3, 0, (Δt^2)/2], [(Δt^2)/2, 0, Δt, 0], [0, (Δt^2)/2, 0, Δt]] * σ_a²其中σ_a是过程加速度的标准差。但实际调试中直接调σ_a太抽象。更实用的方法是先固定Q为diag([q1, q1, q2, q2])然后用目标实际运动数据反推。怎么做录一段目标匀速直线运动的视频用OpenCV提取真值轨迹如用高精度标定板运行卡尔曼滤波观察滤波输出与真值的残差。如果残差在速度分量上系统性偏大说明q2太小模型太“自信”没给加速度留够空间如果位置分量平滑过度跟不上真实转弯说明q1太小。我最终在STM32云台上用的Q是diag([0.01, 0.01, 0.005, 0.005])单位是m²和(m/s)²对应室内小车约±0.3 m/s²的加速度波动。实操心得Q的调优是“手感活”。不要一上来就调得很小试图“保精度”那样滤波器会拒绝任何测量更新变成纯预测一遇到遮挡就彻底失联。我的口诀是“宁可Q稍大不可Q过小”。大Q让滤波器更“谦逊”愿意听测量的话小Q让它“固执”容易跟丢。3.3 观测模型H与观测噪声R把摄像头坐标“翻译”成状态观测模型H将状态向量映射到你能直接测量的量。摄像头给你的是像素坐标(u, v)而状态x是物理坐标米。这里需要相机标定参数。最简情况假设你已用OpenCV标定相机获得内参矩阵K3×3和畸变系数。目标在图像平面上的投影为[u; v; 1] ≈ K * [R|t] * [X; Y; Z; 1] (世界坐标系)但云台追踪通常用“归一化平面坐标”简化。如果你的云台俯仰/偏航角度已知且目标Z深度近似恒定如桌面追踪可建立线性映射z H * x v其中z [u, v]^T是2维观测向量v是观测噪声。H矩阵为H [[1, 0, 0, 0], // u 只与 x_position 相关经标定转换 [0, 1, 0, 0]] // v 只与 y_position 相关即H是2×4矩阵只取状态x的前两维位置。这意味着我们假设摄像头坐标(u,v)直接正比于物理位置(x,y)比例系数已隐含在标定过程中。R矩阵则是观测噪声协方差对角元是u、v坐标的方差。我用USB摄像头在稳定光照下测得单帧检测框中心坐标标准差约±2.5像素故设R diag([6.25, 6.25])。关键细节H矩阵的正确性决定了整个滤波的效果。如果H错了比如你误以为v坐标对应x_position那滤波器会永远学不会。务必用标定板验证让标定板在已知物理位置移动记录(u,v)与(x,y)的对应关系拟合出H。别信理论值信实测数据。3.4 卡尔曼五步从公式到C/Python代码的逐行映射现在把所有部件组装起来。卡尔曼滤波循环就五步每一步都有明确的物理意义和代码对应Step 1: 预测Predict——“我猜它现在在哪”// C伪代码STM32 HAL库 x_pred F * x_est; // 状态预测用模型推演 P_pred F * P_est * F_T Q; // 协方差预测不确定性传播过程噪声x_est是上一时刻最优估计带*号的x̂ₖ₋₁F_T是F的转置P_est是上一时刻估计协方差不确定性大小这一步不依赖新测量纯靠模型。云台在此刻就可以根据x_pred的(x,y)部分粗略调整舵机角度降低延迟。Step 2: 计算卡尔曼增益K —— “这次测量值我该信几分”S H * P_pred * H_T R; // 创新协方差预测不确定性 测量噪声 K P_pred * H_T * inv(S); // 卡尔曼增益最优权重S是创新Innovation的协方差即预测与测量之差的不确定性。inv(S)在嵌入式上不能直接求逆要用Cholesky分解或针对2×2矩阵的手动公式。我STM32用的是手动公式对2×2矩阵[[a,b],[c,d]]逆矩阵为1/(ad-bc) * [[d,-b],[-c,a]]。K是核心它是一个4×2矩阵状态维×观测维。K的每一行告诉你为了修正某个状态分量如x_position应该从观测残差(z - H*x_pred)中取多少比例。Step 3: 更新状态Update—— “把测量信息融合进来”y z - H * x_pred; // 创新残差测量值 - 预测值 x_est x_pred K * y; // 状态更新预测 增益×残差y是2维向量即(u_measured - u_predicted, v_measured - v_predicted)。K * y是4维修正量直接加到x_pred上得到最终估计x_est。这就是你送给舵机控制算法的“最可信位置”。Step 4: 更新协方差P —— “融合后我的不确定性变多少了”I eye(4); // 4×4单位阵 P_est (I - K * H) * P_pred; // 协方差更新不确定性收缩(I - K*H)是“收缩因子”。K越大收缩越狠P越小表示你这次融合后信心越足。P_est用于下一帧的预测步形成闭环。Step 5: 输出与应用 —— “把结果变成舵机动作”// 提取位置和速度 float target_x x_est[0]; float target_y x_est[1]; float vel_x x_est[2]; float vel_y x_est[3]; // 转换为云台角度需提前标定云台电机角度与像素的映射关系 float pan_angle map_pixel_to_angle(target_x, image_width); float tilt_angle map_pixel_to_angle(target_y, image_height); // 发送给舵机PWM信号 set_servo_angle(PAN_SERVO, pan_angle); set_servo_angle(TILT_SERVO, tilt_angle);map_pixel_to_angle函数是关键桥梁。它不是线性比例因为云台转动是非线性的小角度灵敏大角度迟钝。我用多项式拟合angle a*u² b*u c系数通过实测云台在不同像素位置对应的舵机角度得到。实操陷阱初学者常把x_est直接当像素坐标用这是致命错误。x_est是物理坐标米必须通过相机模型或标定查表转换为图像坐标(u,v)再映射到舵机角度。我见过太多人卡在这一步云台乱转还以为是滤波参数不对。3.5 STM32与OpenCV的协同架构谁干啥边界在哪一个常见误区是把所有计算塞进STM32。实际上合理分工才能发挥各自优势OpenCV树莓派/PC端负责图像采集与预处理去噪、二值化目标检测YOLOv5/v8、Haar级联、颜色分割输出原始检测框中心坐标(u, v)及置信度可选发送给STM32的串口指令如“目标已确认”、“目标丢失”STM32主控MCU负责接收OpenCV发来的(u, v)坐标通过UART或SPI执行卡尔曼滤波五步全部在C语言中实现用定点数或float根据x_est计算舵机PWM占空比控制舵机驱动芯片如PCA9685处理紧急停止、限位保护等底层安全逻辑为什么这样分因为OpenCV擅长图像计算但实时性差STM32中断响应快微秒级但算力弱。把滤波放在STM32确保从收到坐标到输出PWM在1ms内完成避免视觉延迟累积。我用STM32F407浮点运算足够跑4维卡尔曼内存也够存P矩阵4×416 float。经验技巧UART通信要加简单协议。我用0xAA u_high u_low v_high v_low checksum帧格式STM32收到后校验丢弃错误帧。别用printf太慢。OpenCV端用ser.write()STM32端用HAL_UART_Receive_IT()加环形缓冲区避免丢帧。4. 从零到一一个可运行的STM32OpenCV云台追踪完整实操4.1 硬件准备清单与关键选型理由别跳过这一步。硬件不匹配再好的算法也是空中楼阁。这是我反复验证过的最小可行配置主控MCUSTM32F407VGT6理由168MHz主频浮点单元FPU原生支持256KB Flash/64KB RAM足够存滤波变量和PID参数。比F1系列强太多比F7又便宜。注意选LQFP100封装引脚够用。摄像头OV2640模组带FIFO理由200万像素支持JPEG压缩输出通过DCMI接口直接连STM32省去树莓派。但DCMI对时序要求严新手易翻车。更推荐方案USB摄像头如罗技C270 树莓派Zero 2 W用OpenCV处理UART发坐标给STM32。成本略高但开发效率提升300%。舵机MG996R金属齿×2理由扭矩大11kg·cm价格低兼容性强。注意它工作电压是4.8~6.6V别直接用STM32的3.3V IO驱动必须用舵机驱动板如Adafruit PCA9685或MOSFET电路隔离。云台结构3D打印双轴云台支架理由自己设计确保俯仰/偏航轴正交减少耦合误差。我用SolidWorks建模壁厚2mm打样后实测晃动小于0.5°。别用淘宝廉价云台齿轮间隙会导致“爬行”现象滤波也救不了。电源12V 2A开关电源 AMS1117-5.0稳压模块理由舵机瞬时电流大必须独立供电。AMS1117给STM32和PCA9685供5V纹波小比DC-DC更稳。关键提醒所有GND必须单点共地我曾因STM32、PCA9685、摄像头各自接地引入共模噪声导致舵机嗡嗡响。用一根粗铜线把所有模块的GND焊接到PCB同一个焊盘上。4.2 OpenCV端检测与坐标提取的稳健实现OpenCV代码的核心是鲁棒性不是精度。目标是稳定输出(u,v)而不是追求99.9% mAP。import cv2 import numpy as np import serial import time # 初始化串口连接STM32 ser serial.Serial(/dev/ttyUSB0, 115200, timeout0.01) # 加载YOLOv5s模型轻量版 net cv2.dnn.readNet(yolov5s.onnx) # ONNX格式免编译 net.setPreferableBackend(cv2.dnn.DNN_BACKEND_OPENCV) net.setPreferableTarget(cv2.dnn.DNN_TARGET_CPU) # 定义目标类别例如person的index0 TARGET_CLASS 0 CONF_THRESHOLD 0.5 NMS_THRESHOLD 0.4 def detect_and_send(frame): h, w frame.shape[:2] # 预处理缩放至640x480归一化 blob cv2.dnn.blobFromImage(frame, 1/255.0, (640, 480), (0,0,0), swapRBTrue, cropFalse) net.setInput(blob) outputs net.forward(net.getUnconnectedOutLayersNames()) # 后处理NMS过滤 boxes [] confidences [] for output in outputs: for detection in output: scores detection[5:] class_id np.argmax(scores) confidence scores[class_id] if class_id TARGET_CLASS and confidence CONF_THRESHOLD: center_x, center_y int(detection[0] * w), int(detection[1] * h) boxes.append([center_x, center_y, int(detection[2]*w), int(detection[3]*h)]) confidences.append(float(confidence)) # NMS去重 indices cv2.dnn.NMSBoxes(boxes, confidences, CONF_THRESHOLD, NMS_THRESHOLD) if len(indices) 0: # 取置信度最高的框 i indices[0] x, y, w_box, h_box boxes[i] # 发送中心坐标u,v到STM32 u, v x, y # 协议0xAA u_high u_low v_high v_low checksum packet bytearray([0xAA, (u8)0xFF, u0xFF, (v8)0xFF, v0xFF]) checksum sum(packet) 0xFF packet.append(checksum) ser.write(packet) return True, (u, v) else: # 发送丢失信号uv0xFFFF packet bytearray([0xAA, 0xFF, 0xFF, 0xFF, 0xFF, 0xFE]) ser.write(packet) return False, (0, 0) # 主循环 cap cv2.VideoCapture(0) cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640) cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480) cap.set(cv2.CAP_PROP_FPS, 30) while True: ret, frame cap.read() if not ret: break # 显示原始帧 cv2.imshow(Camera, frame) # 检测并发送 is_detected, (u, v) detect_and_send(frame) if is_detected: # 在画面上画框和中心点 cv2.circle(frame, (u, v), 5, (0,255,0), -1) if cv2.waitKey(1) 0xFF ord(q): break cap.release() cv2.destroyAllWindows()注意事项ONNX模型比PyTorch快3倍树莓派Zero 2 W跑ONNX推理约120ms/帧足够实时。别用原始.pt文件。NMS阈值设为0.4太高会漏检太低产生多框。我实测0.4在多人场景下ID Switch最少。发送丢失信号很重要STM32收到0xFFFF就知道要启动遮挡预测模式P矩阵开始扩散而不是死等。4.3 STM32端卡尔曼滤波的C语言实现与优化以下是核心滤波代码基于HAL库已去除无关外设初始化#include main.h #include math.h // 卡尔曼滤波状态变量全局 float x_est[4] {320.0f, 240.0f, 0.0f, 0.0f}; // 初始位置在画面中心速度为0 float P_est[16] {100.0f, 0.0f, 0.0f, 0.0f, 0.0f, 100.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f}; // 初始不确定性较大 // 模型参数根据实际帧率动态更新 float F[16]; // 4x4状态转移矩阵 float Q[16] {0.01f, 0.0f, 0.0f, 0.0f, 0.0f, 0.01f, 0.0f, 0.0f, 0.0f, 0.0f, 0.005f, 0.0f, 0.0f, 0.0f, 0.0f, 0.005f}; float H[8] {1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f}; // 2x4观测矩阵 float R[4] {6.25f, 0.0f, 0.0f, 6.25f}; // 2x2观测噪声 // 矩阵运算函数精简版只实现所需操作 void mat_mult(float* A, int rowsA, int colsA, float* B, int colsB, float* C) { for(int i0; irowsA; i) { for(int j0; jcolsB; j) { C[i*colsBj] 0.0f; for(int k0; kcolsA; k) { C[i*colsBj] A[i*colsAk] * B[k*colsBj]; } } } } void mat_add(float* A, float* B, int size, float* C) { for(int i0; isize; i) C[i] A[i] B[i]; } void mat_sub(float* A, float* B, int size, float* C) { for(int i0; isize; i) C[i] A[i] - B[i]; } // 2x2矩阵求逆手动公式 void mat_inv2x2(float* A, float* invA) { float det A[0]*A[3] - A[1]*A[2]; if(fabsf(det) 1e-6f) return; // 防止除零 invA[0] A[3]/det; invA[1] -A[1]/det; invA[2] -A[2]/det; invA[3] A[0]/det; } // 卡尔曼滤波主函数 void kalman_filter_update(float u, float v) { // Step 1: Predict float x_pred[4], P_pred[16], F_T[16], temp1[16], temp2[16]; // 计算F_TF转置 for(int i0; i4; i) { for(int j0; j4; j) { F_T[i*4j] F[j*4i]; } } // x_pred F * x_est mat_mult(F, 4, 4, x_est, 4, x_pred); // P_pred F * P_est * F_T Q mat_mult(F, 4, 4, P_est, 4, temp1); // F * P_est mat_mult(temp1, 4, 4, F_T, 4, P_pred); // F * P_est * F_T mat_add(P_pred, Q, 16, P_pred); // Q // Step 2: Compute Kalman Gain K float S[4], H_T[8], temp3[8], temp4[16], temp5[16], K[8]; // H_T (4x2) for(int i0; i4; i) { for(int j0; j2; j) { H_T[i*2j] H[j*4i]; } } // S H * P_pred * H_T R mat_mult(H, 2, 4, P_pred, 4, temp3); // H * P_pred mat_mult(temp3, 2, 4, H_T, 2, S); // H * P_pred * H_T mat_add(S, R, 4, S); // R // K P_pred * H_T * inv(S) mat_mult(P_pred, 4, 4, H_T, 2, temp4); // P_pred * H_T float S_inv[4]; mat_inv2x2(S, S_inv); mat