1. 先从上一篇的CV模型说起再看为什么要换如果你看过这个系列的上一篇应该还有印象我们在最基础的目标跟踪例子里用了一个四维状态向量位置加速度也就是常说的恒定速度模型CVConstant Velocity。那个模型数学形式非常漂亮状态转移矩阵是线性的测量方程也是线性的卡尔曼滤波KF可以直接套公式几分钟就能跑通一个像模像样的跟踪Demo。当时的推导过程很顺预测、更新、融合一气呵成。但用过一段时间之后你会发现CV模型在真实场景里有一个绕不开的尴尬它假设目标一直在走直线。一旦前车开始转弯或者行人拐了个弯跟踪框就开始飘位置估计出现明显滞后协方差也会慢慢变得不自信。原因不复杂——CV模型的运动方程里根本没有描述“转弯”的变量所有轨迹曲率都被当成过程噪声“吸收”了。噪声一旦调大滤波器的状态就会变得模糊精度和稳定性的矛盾开始显现。所以到了这一篇重点就是两件事一是把模型从CV升级到CTRV恒定转弯率和速度模型Constant Turn Rate and Velocity二是把传感器从单纯一个激光雷达扩展到雷达和激光雷达同时参与并处理好它们各自测量模型截然不同的数学形式。简单说这篇的核心不是“再写一遍卡尔曼滤波”而是搞清楚什么时候必须放弃线性KF改用扩展卡尔曼滤波EKF以及两种传感器数据到底应该怎么融合才能稳定、精度高、不出幺蛾子。这篇内容适合正在做自动驾驶感知、机器人定位或者任何涉及多传感器目标跟踪的朋友。默认你已经理解上一篇的基础KF流程比如状态预测、测量更新、卡尔曼增益这些概念都见过。如果这些名词还有点模糊建议先回头翻翻之前的推导再来读这节内容会更顺。真没看过也不慌下面每一步我都会把公式的含义用大白话拆开讲。2. 从CV到CTRV状态向量变了模型发生了什么变化2.1 CV模型的局限在哪里先把上一篇的CV模型拉出来看。它的状态向量一般写成[ x [p_x, p_y, v_x, v_y]^T ]也就是水平位置和速度分量。状态转移方程是[ p_{x,k1} p_{x,k} v_{x,k} \cdot \Delta t ] [ p_{y,k1} p_{y,k} v_{y,k} \cdot \Delta t ] [ v_{x,k1} v_{x,k} ] [ v_{y,k1} v_{y,k} ]这个模型默认目标速度矢量不变轨迹自然是直线。真实世界里哪有那么多直线车辆转弯时速度方向在变速度大小可能大致不变CV模型完全无法描述“速度方向变化”这件事只能把这种变化归咎于过程噪声Q矩阵里的加速度不确定性。这里有个很微妙的工程问题Q矩阵是滤波器对模型“不准确程度”的信任度。你把Q调大滤波器就更依赖测量值结果就是估计值抖动你把Q调小滤波器更相信模型预测但目标一旦真转弯你就会看到跟踪轨迹在弯道内侧“抄近路”等测量值把它拉回来时已经晚了。用CV模型做转弯目标跟踪本质上是在“抖得厉害”和“反应迟钝”之间二选一怎么调都不舒服。2.2 CTRV模型的状态向量设计CTRV模型把“转弯”作为一个显式状态量放进来状态向量从四维变成五维[ x [p_x, p_y, v, \psi, \dot{\psi}]^T ]注意变化点速度不再分x、y分量而是变成一个标量速率 ( v )再加一个朝向角 ( \psi ) 和朝向角的角速度 ( \dot{\psi} )也叫偏航率yaw rate。其中( v )目标的合速度大小直接描述“走得有多快”( \psi )航向角描述此时朝哪个方向走( \dot{\psi} )航向角的变化率也就是转弯的快慢这样设计的好处在于模型里天然包含“转弯率”这一个自由度目标转弯时不再是“噪声带来的干扰”而是模型本身就具备描述这种运动的能力。从几何直观上理解CV模型走的是一条直线CTRV模型走的是圆弧。当 ( \dot{\psi} ) 接近0时圆弧退化成直线自动回到CV模型的行为。这也是为什么CTRV在实际工程中如此常用——它覆盖了直线和转弯是一个更通用的运动假设。2.3 状态转移方程的两种写法CTRV的状态转移方程需要注意一个关键分支当角速度 ( \dot{\psi} ) 的绝对值非常小近似直线运动和较大明显转弯时位置更新的数学形式是不同的。先说明显的转弯情况。状态更新公式为[ p_{x,k1} p_{x,k} \frac{v}{\dot{\psi}} \left[ \sin(\psi_k \dot{\psi}k \Delta t) - \sin(\psi_k) \right] ] [ p{y,k1} p_{y,k} \frac{v}{\dot{\psi}} \left[ -\cos(\psi_k \dot{\psi}k \Delta t) \cos(\psi_k) \right] ] [ v{k1} v_k ] [ \psi_{k1} \psi_k \dot{\psi}k \Delta t ] [ \dot{\psi}{k1} \dot{\psi}_k ]看着有点吓人实际上就是在算在角速度作用下目标沿着圆弧走了多远然后把这个弧长分解到x、y两个方向。你可以把 ( v / \dot{\psi} ) 理解成转弯半径 ( R )公式的本质就是[ \Delta x R \cdot \Delta \sin(\psi) ] [ \Delta y R \cdot \Delta (-\cos(\psi)) ]如果 ( \dot{\psi} ) 非常小比如小于某个阈值上面这个公式的除法会产生数值不稳定因为分母趋近于0。工程上的惯例是直接用直线近似[ p_{x,k1} p_{x,k} v \cos(\psi_k) \Delta t ] [ p_{y,k1} p_{y,k} v \sin(\psi_k) \Delta t ]这个分支判断非常重要。很多第一次实现CTRV的人都会在这里踩坑没有加分支直接用带分母的公式结果目标明明在走直线跟踪结果却一会儿跳一下。我自己的经验是阈值取 ( 0.001 ) 到 ( 0.01 ) rad/s 之间比较合理太小容易触发数值问题太大会把轻微转弯也当成直线处理。具体取多少要结合传感器帧率和目标运动特性这个后面在调参章节还会细说。3. 从KF到EKF非线性问题是怎么冒出来的3.1 大一统KF还能继续用吗上一篇里我们愉快地用了KF全程都是矩阵乘法没有一分一毫的高级数学。能这么干的前提是状态转移和测量方程都是线性的。线性是什么意思大白话说就是“输入变化多少输出就成比例变化多少”不存在角度、平方、除法这种弯弯绕绕的运算。现在看看CTRV的状态转移方程你再平移一下视线里面有 ( \sin(\psi) )、( \cos(\psi) )而且如果转弯率不为零还有 ( v / \dot{\psi} ) 这种除法。这些都不是线性运算。KF的经典推导过程中有一个关键假设高斯分布经过线性变换后还是高斯分布。如果变换本身是非线性的那高斯分布经过变换后就不再是高斯分布了KF的公式推导地基直接塌了。这就是EKF登上舞台的原因。EKF的思路粗暴但有效在某个工作点附近对非线性函数做一阶泰勒展开把非线性函数局部线性化然后再套用标准KF的框架。它不求全局准确只求在当前估计点附近近似准确——像你站在一个弯曲的山坡上你用脚下这一点的切线来近似整个山坡的形状坡度变化不太剧烈时这个近似够用。3.2 非线性来自两个地方在雷达和激光雷达融合的例子里非线性来源有两处。第一处是状态转移方程。CTRV模型里的 ( \sin ) 和 ( \cos ) 是状态变量的函数所以预测步骤必须线性化。具体操作就是求状态转移函数对状态向量的雅可比矩阵。第二处是雷达的测量方程。激光雷达的测量值本来就是笛卡尔坐标系下的 ( (p_x, p_y) )从状态向量到测量值就是一个线性映射直接写一个矩阵就能搞定。但雷达不一样雷达测量得到的是距离 ( \rho )、方位角 ( \varphi )、距离变化率 ( \dot{\rho} )多普勒速度。这个测量向量和状态向量之间是极坐标到笛卡尔坐标的转换关系[ \rho \sqrt{p_x^2 p_y^2} ] [ \varphi \arctan2(p_y, p_x) ] [ \dot{\rho} \frac{p_x v \cos(\psi) p_y v \sin(\psi)}{\sqrt{p_x^2 p_y^2}} ]这三个公式里既有平方根又有反正切还有除法全是非线性运算。所以雷达的测量更新步骤必须用EKF而激光雷达的测量更新步骤可以继续用标准KF的线性版本。这也引出了后面融合策略的关键点两种传感器不能“一刀切”地套同一个流程。3.3 前置知识雅可比矩阵怎么算EKF核心步骤其实只是换了两处预测步骤里的 ( A ) 矩阵换成雅可比矩阵 ( F_j )测量更新里的 ( H ) 矩阵换成雅可比矩阵 ( H_j )。雅可比矩阵说白了就是“每个输出分量对每个输入分量的偏导数”。如果状态向量维度是 ( n )测量向量维度是 ( m )那么测量雅可比矩阵 ( H_j ) 就是一个 ( m \times n ) 矩阵第 ( i ) 行第 ( j ) 列元素就是第 ( i ) 个测量分量对第 ( j ) 个状态分量的偏导。这里给一个最典型的做法后面实操部分也会用到。以雷达测量模型为例状态向量是 ( [p_x, p_y, v, \psi, \dot{\psi}] )测量向量是 ( [\rho, \varphi, \dot{\rho}] )可以手推或者用符号计算工具得到 ( H_j ) 的解析式。虽然推导过程中涉及一堆链式法则但最终落实成代码只是一些加减乘除不会出现让人头疼的复杂运算。真正需要留神的是 ( \rho ) 可能为0时的除零问题这个在常见问题章节会专门讲。4. 雷达和激光雷达的测量模型有什么不同4.1 激光雷达从状态到测量就一步线性变换激光雷达在目标跟踪里的角色很直白它直接给出目标在笛卡尔坐标系下的位置坐标。对于CTRV状态向量 ( [p_x, p_y, v, \psi, \dot{\psi}] )激光雷达的测量向量是[ z_{lidar} [p_x, p_y]^T ]对应的测量矩阵 ( H ) 就是[ H \begin{bmatrix} 1 0 0 0 0 \ 0 1 0 0 0 \end{bmatrix} ]看到没有这就是个“挑出前两个元素”的矩阵线性得不能再线性。所以激光雷达的测量更新可以直接用普通KF公式完全走标准流程。实际工程里激光雷达的测量噪声一般建模为高斯白噪声测量噪声协方差矩阵 ( R ) 取对角阵x和y方向的噪声方差根据传感器手册实测填入。有一点很多人忽略激光雷达和雷达的量测单位不一样融合时状态向量的单位必须统一。如果激光雷达输出的是米雷达输出的距离也是米那没问题但如果某个传感器输出的是毫米或者公里那就要做单位换算。这种低级错误不出现在公式里却经常出现在集成的第一版代码里找半天Bug发现是单位不统一。4.2 雷达极坐标系下的三个测量量雷达的测量模型复杂一些。它测的是目标相对于雷达的距离、方位角和距离变化率也就是极坐标系下的信息。三个测量量分别是( \rho )径向距离即目标到雷达的直线距离( \varphi )方位角目标相对雷达的朝向通常以雷达正前方为0度逆时针为正( \dot{\rho} )径向速度距离变化率目标沿雷达视线的相对速度分量也就是多普勒速度重点理解一下第三个量。雷达的多普勒效应能直接测出目标靠近雷达的径向速度但测不出垂直于雷达视线的横向速度。这个特性其实是雷达的一大优势即使目标位置变化很小多普勒速度也能提供丰富的运动信息。代价则是测量方程极其非线性必须做极坐标转换。4.3 两种传感器的优势互补融合雷达和激光雷达不是“多一个传感器多一份安心”那么简单而是它们提供的信息几乎正交。激光雷达精度高、分辨率好尤其在近程和中程范围内位置测量非常准对目标轮廓的刻画能力是雷达比不了的。但激光雷达有两个明显弱点一是恶劣天气雨、雪、雾下性能衰减严重二是对远程目标点云稀疏检出率下降。雷达的功耗小、作用距离远不受光照和天气影响多普勒速度信息更是激光雷达没有的。它的弱点是角度分辨率低横向位置测量精度远不如激光雷达。两者融合后等于用激光雷达的精准位置校正雷达的横向误差用雷达的多普勒信息增强对目标运动状态的感知再用雷达在恶劣天气和远距离场景兜底。这也是为什么几乎所有量产级自动驾驶方案都采用多传感器融合架构而不是押注单一传感器。工程上最朴素的融合策略是两个传感器各自独立做测量更新先更新一个再更新另一个效果等同融合实现简单后面会展开讲。5. 实操EKF预测、雷达更新、激光雷达更新的完整实现5.1 初始化与状态变量约定动手写代码前先把约定讲清楚。这里采用一个最常见做法用一个统一的EKF类管理状态和协方差针对不同传感器调用同一个预测函数针对不同测量模型调用不同的更新函数。初始化时由于这是一个新的跟踪目标还没有历史轨迹需要用第一帧测量来“播种”状态。如果第一帧来自激光雷达状态的位置直接用激光雷达的 ( (p_x, p_y) ) 设置速度和航向角没有直接测量值通常置0或者用后续几帧差分估计。初始航向角如果为0协方差里对应的不确定性要设大一些防止滤波器自信地认为朝向绝对正确。如果第一帧来自雷达用距离和方位角反算位置 ( p_x \rho \cos\varphi )( p_y \rho \sin\varphi )但注意这种转换会引入交叉相关性如果条件允许加上这一步的雅可比传播会更严谨。速率 ( v ) 可以用距离变化率 ( \dot{\rho} ) 做粗略估计航向角 ( \psi ) 先用方位角 ( \varphi ) 近似等下一帧测量再调整。把初始协方差的对应元素设大给滤波器留足修正空间。这里有一个工程上普遍认可的经验初始协方差矩阵别抠得太紧。宁可让滤波器一开始“自信不足”多依赖后续测量收敛也不要一开始就“过度自信”导致几帧之内都拉不回来。初始协方差抖大些多帧后滤波器自然会收敛。5.2 核心代码预测步骤预测步骤的代码并不复杂核心是把状态转移函数算出来然后把雅可比矩阵和过程噪声协方差代进标准公式。直接用 Python 伪代码演示其中关键部分。import numpy as np def predict(self, dt): # 状态向量: [px, py, v, psi, psi_dot] px, py, v, psi, psi_dot self.x # 角速度接近0时用直线近似避免除法不稳定 if abs(psi_dot) 0.001: px_new px v * np.cos(psi) * dt py_new py v * np.sin(psi) * dt psi_new psi else: px_new px (v / psi_dot) * (np.sin(psi psi_dot * dt) - np.sin(psi)) py_new py (v / psi_dot) * (-np.cos(psi psi_dot * dt) np.cos(psi)) psi_new psi psi_dot * dt self.x np.array([px_new, py_new, v, psi_new, psi_dot]) # 计算状态转移雅可比矩阵 F_j F_j self.compute_F_j(dt, px, py, v, psi, psi_dot) self.P F_j self.P F_j.T self.Q上面这段代码只展示了状态和协方差的更新。compute_F_j是状态转移函数的雅可比矩阵公式比较长但在符号推导工具或者手推之后写出来也就是矩阵代值。这里特别提醒一下psi的取值问题很多直接用的人会发现预测几步之后角度跑到几百上千度甚至超过 ( 2\pi ) 产生数值问题。所以每次更新完psi建议顺手做一个角度归一化保证它在 ( (-\pi, \pi] ) 区间内。5.3 核心代码激光雷达更新激光雷达的更新就是标准的KF线性更新代码也非常直接。def update_lidar(self, z): # 测量向量是 [px, py] H np.array([[1, 0, 0, 0, 0], [0, 1, 0, 0, 0]]) R self.R_lidar # 2x2对角矩阵 y z - H self.x # 创新项 S H self.P H.T R # 创新协方差 K self.P H.T np.linalg.inv(S) # 卡尔曼增益 self.x self.x K y self.P (np.eye(5) - K H) self.P注意最后一行更稳定的做法是用I - K H乘以P这样在数值上维持矩阵对称性更好在多帧迭代后不容易出现协方差矩阵不对称导致的发散。如果你发现滤波器跑了一段时间协方差矩阵不对称了八成是这里没用约瑟夫形式的更新。5.4 核心代码雷达更新雷达更新的难点全在测量雅可比矩阵 ( H_j ) 的计算。用符号推导工具把前面给的三个测量函数对五个状态量求偏导得到的是一个 ( 3 \times 5 ) 矩阵。为了照顾手写实现的读者这里补充一个等价但实现更简单的小技巧可以先用测量函数算出预测的测量值再用数值差分近似雅可比。这个技巧强烈推荐在初期调通流程时使用因为手推解析式容易犯错而数值差分只要测量函数写对就不会错。等整个流程跑通了再把解析式替换进去性能会好很多。核心更新代码如下所示。def update_radar(self, z): z_pred self.radar_measurement_function(self.x) H_j self.compute_radar_Jacobian(self.x) # 3x5矩阵 R self.R_radar # 3x3对角矩阵对应 [rho, phi, rho_dot] 噪声 y z - z_pred # 角度归一化把 y[1]角度差规范到 (-pi, pi] y[1] wrap_to_pi(y[1]) S H_j self.P H_j.T R K self.P H_j.T np.linalg.inv(S) self.x self.x K y self.P (np.eye(5) - K H_j) self.P这里的wrap_to_pi极度关键。如果不做角度归一化当真实角度从 ( 179^\circ ) 变到 ( -179^\circ ) 时创新项会变成 ( -358^\circ )滤波器会完全懵掉明明只差了2度却像是差了将近一圈。这个Bug的效果是跟踪轨迹突然跳变甚至直接发散排查起来非常隐蔽。5.5 融合策略两种更新的组合顺序融合的核心套路很简单雷达和激光雷达的测量是彼此独立的异步事件所以在一个周期内我们可以先做一次预测然后按照传感器到达顺序依次执行各自的更新。这背后的数学依据是如果两个测量噪声相互独立顺序地用卡尔曼滤波做两次更新等价于一次性把两个测量拼成一个大的测量向量做一次联合更新。但拼成大向量时需要处理“雷达是非线性、激光雷达是线性”的异构问题公式上麻烦不少。实际工程里更流行的做法就是顺序更新代码清晰逻辑也直白。一个典型调用流程长这样ekf.predict(dt) if lidar_measurement_available: ekf.update_lidar(z_lidar) if radar_measurement_available: ekf.update_radar(z_radar)注意两次更新用的是同一个预测后的状态和协方差。这种“先预测一次然后多个传感器依次更新”的模式非常稳定在很多开源自动驾驶项目中都是标准做法。顺序更新的另一个好处是雷达和激光雷达的帧率不一样也不会干扰逻辑——哪个传感器的数据先到就先更新哪个。6. 调参与踩坑从第一次跑通到稳定跟踪6.1 过程噪声协方差矩阵Q怎么设Q矩阵在CTRV模型下比CV模型多一个维度对应的是状态向量中“模型不确定量”的方差。这里有两个关键的噪声源直线加速度噪声 ( \sigma_a^2 )描述目标速度发生突变的概率角加速度噪声 ( \sigma_{\dot{\psi}}^2 )描述目标转弯率发生突变的概率Q矩阵的严格推导需要对状态转移函数做噪声传播分析但工程上有个比较实用的近似。把Q写成类似这样的形式具体推导过程常见于各类自动驾驶课程和资料中[ Q \begin{bmatrix} \frac{1}{4} \sigma_a^2 \Delta t^4 \cos^2\psi \frac{1}{4} \sigma_a^2 \Delta t^4 \cos\psi \sin\psi \cdots \ \vdots \ddots \vdots \end{bmatrix} ]看着矩阵元素很多实际写代码的时候可以借鉴一个“偷懒但能跑”的思路先只填对角元素把Q简化成一个对角阵然后根据真实场景调大小。如果你发现跟踪轨迹抖动厉害多半是Q偏大如果你发现跟踪滞后、对转弯反应迟钝多半是Q偏小。Q的调参没有银弹只能靠场景和数据做实验但对角阵起步、逐步调整是最高效的路径。给一个参考初始值行人目标( \sigma_a^2 ) 取 ( 1.0 \sim 2.0 )( \sigma_{\dot{\psi}}^2 ) 取 ( 0.5 \sim 1.0 )车辆目标( \sigma_a^2 ) 取 ( 0.1 \sim 0.5 )( \sigma_{\dot{\psi}}^2 ) 取 ( 0.3 \sim 0.8 )。这些值没有普适性但作为起点足够。6.2 雷达测量噪声R怎么定雷达的 ( R ) 矩阵通常是一个3x3对角阵距离测量噪声典型值 ( 0.05 \sim 0.2 ) 米不同雷达差异较大以手册为准方位角测量噪声典型值 ( 0.01 \sim 0.05 ) 弧度约0.6度到3度距离变化率噪声典型值 ( 0.1 \sim 0.5 ) 米/秒这里有一个常见误区有人会把R设得很小希望滤波器完全相信雷达的测量。但在雷达测量模型是非线性的情况下R过小意味着把创新项里的非线性误差全当作真实误差来修正反而容易导致滤波发散。我见过不少项目因为R设得过小EKF跑了几百帧协方差就崩了。正确做法是参考传感器手册的实测标准差不要凭感觉压低。激光雷达的R相对好设位置测量精度通常在几厘米量级x和y方向各取 ( 0.01 \sim 0.04 ) 矩形比较稳妥。6.3 常见问题和排查技巧实践过程中我整理过一个排查清单按出现频率排序如下。角度跳变导致轨迹跳变特征是一条跟踪轨迹正常跑着突然向侧面跳一下然后又弹回来。这大概率是创新项里的角度差没有归一化。解决方案是写一个wrap_to_pi函数在计算创新项后对角度差做归一化。排查时可以打印原始角度差和归一化后的角度差一眼就能看出问题。协方差发散或负定多出现在EKF更新之后协方差矩阵不是正定的情况。常见原因一是I - K H的形式没有写成约瑟夫形式导致数值不对称累积二是测量雅可比矩阵H_j计算错误单位或符号搞反。排查时打印P的迹和特征值看是否单调变化。雷达的 ( \rho ) 为0导致除零极坐标转换里如果目标正好在雷达正下方或正前方很近的地方( \rho ) 可能非常小。这时计算雅可比矩阵时会出现除以 ( \rho ) 的操作直接触发除零错误。工程上加一个极小值保护比如 ( \rho_{\min} 0.01 )小于该值就用线性近似跳过这一步。多传感器融合后轨迹不如单激光雷达如果融合后精度比单个激光雷达还差问题大概率出在雷达的初始化和噪声参数上。先用纯激光雷达跑一遍作为基线再逐步加入雷达对比每个环节的状态输出定位是雷达哪个量把状态带偏了。预测后协方差变小但是状态没变对这种情况通常是F_j矩阵计算错误尤其容易出现在sin和cos的参数里忘记加psi_dot * dt。建议先用一个匀速直线运动的目标做测试CTRV模型的预测结果应该退化成CV模型的行为如果这个一致性测试都过不了说明状态转移函数有问题。6.4 验证结果NIS一致性检验跑完整个融合流程后光看轨迹“好像还挺像样”是不够的。工程上有个更严谨的指标叫归一化创新平方NIS定义是[ NIS y^T S^{-1} y ]其中 ( y ) 是创新项( S ) 是创新协方差。如果滤波器调参正确NIS应该符合卡方分布。查卡方分布表可以得到一个合理的置信区间比如95%置信度下雷达测量维度3的NIS阈值大约在 ( [0.35, 7.8] ) 之间激光雷达测量维度2的NIS阈值大约在 ( [0.05, 5.99] ) 之间。NIS过大说明滤波器对测量过于“惊讶”——要么R太小要么模型预测太不准NIS长期偏小说明滤波器过度自信——模型可能在过拟合测量数据。我自己调试时习惯画一条NIS时序曲线看到底是哪些时刻出现了异常这样比盯着轨迹图更能快速定位问题。7. 最后的一些经验分享这个系列写到这里我再分享一点个人感受。很多朋友从CV模型转到CTRV模型时第一感觉是“哇公式变得好复杂”于是下意识地想找现成的库或者干脆死啃论文。我的建议是哪怕最后要接开源库也一定自己手写一遍CTRV的预测函数、雅可比推导和EKF更新。原因特别简单EKF的调试过程基本等于“看你写的公式哪里和真实物理对不上”。如果你没有亲手推过雅可比矩阵你永远不知道错是出在状态转移函数、测量函数还是噪声参数上。你也不会有“看到轨迹跳变第一反应是角度归一化”这种条件反射式的排查能力。这些在真实项目里比会调库重要得多。手写完之后再用开源代码对比验证这时候你才真正“拥有”了这套融合逻辑而不是跑通一个黑盒。下一篇我大概率会继续往深挖一下聊聊无损卡尔曼滤波UKF相比EKF在高非线性场景下的精度优势和采样策略那时候你会发现有了CTRV模型的底子理解UKF就是顺水推舟的事。先把这一篇的代码跑通、数据调顺后面的路自然就清晰了。