企业官网建设流程全解析

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²，对应Δt=0.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, CV）：

x = [x_position, y_position, x_velocity, y_velocity]^T

即4维向量。为什么选这个？因为绝大多数室内移动目标（小车、人、无人机）在短时间尺度（<0.5秒）内，速度变化相对缓慢，加速度可视为噪声。它比纯位置模型（2维）多了速度信息，能预测下一帧大概位置；又比恒加速模型（6维）简单，避免过度拟合噪声。

状态转移矩阵F描述“如果没有外部干扰，状态如何自然演化”。假设帧间隔为Δt（例如OpenCV处理一帧耗时33ms，则Δt=0.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（Δt=0.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标定相机，获得内参矩阵K（3×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×4=16 float）。

经验技巧：UART通信要加简单协议。我用0xAA + u_high + u_low + v_high + v_low + checksum帧格式，STM32收到后校验，丢弃错误帧。别用printf，太慢。OpenCV端用ser.write()，STM32端用HAL_UART_Receive_IT()加环形缓冲区，避免丢帧。

4. 从零到一：一个可运行的STM32+OpenCV云台追踪完整实操

4.1 硬件准备清单与关键选型理由

别跳过这一步。硬件不匹配，再好的算法也是空中楼阁。这是我反复验证过的最小可行配置：

• 主控MCU：STM32F407VGT6
  理由：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, timeout=0.01) # 加载YOLOv5s模型（轻量版） net = cv2.dnn.readNet("yolov5s.onnx") # ONNX格式，免编译 net.setPreferableBackend(cv2.dnn.DNN_BACKEND_OPENCV) net.setPreferableTarget(cv2.dnn.DNN_TARGET_CPU) # 定义目标类别（例如'person'的index=0） 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), swapRB=True, crop=False) 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, (u>>8)&0xFF, u&0xFF, (v>>8)&0xFF, v&0xFF]) checksum = sum(packet) & 0xFF packet.append(checksum) ser.write(packet) return True, (u, v) else: # 发送丢失信号（u=v=0xFFFF） 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 i=0; i<rowsA; i++) { for(int j=0; j<colsB; j++) { C[i*colsB+j] = 0.0f; for(int k=0; k<colsA; k++) { C[i*colsB+j] += A[i*colsA+k] * B[k*colsB+j]; } } } } void mat_add(float* A, float* B, int size, float* C) { for(int i=0; i<size; i++) C[i] = A[i] + B[i]; } void mat_sub(float* A, float* B, int size, float* C) { for(int i=0; i<size; 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_T（F转置） for(int i=0; i<4; i++) { for(int j=0; j<4; j++) { F_T[i*4+j] = F[j*4+i]; } } // 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 i=0; i<4; i++) { for(int j=0; j<2; j++) { H_T[i*2+j] = H[j*4+i]; } } // 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