多彩编程 多彩编程MZPH · CODE BLOG
ARTICLE DETAIL

文章详情

深耕前端与后端开发技术的一线实战笔记与踩坑复盘。

卡尔曼滤波从原理到实践:五个公式、代码实现与调参经验全解析

卡尔曼滤波从原理到实践:五个公式、代码实现与调参经验全解析 卡尔曼滤波这名字做机器人、自动驾驶、无人机或者任何跟传感器打交道的方向的人应该都绕不开。它本质上就是一个“数据融合器”能把一堆有噪声、不完全可靠的测量数据融合成一个相对精准、可信的状态估计。我第一次接触这算法是在做四旋翼无人机姿态解算的时候当时IMU出来的数据抖得没法看卡尔曼滤波一轮调参下来波形干净得让我怀疑是不是传感器坏掉了。从那以后但凡做定位、跟踪、导航我第一反应都是“能不能先上个卡尔曼”。这篇东西我打算直接讲核心不讲那些抄来抄去的数学史。适合刚入门、被一堆矩阵符号劝退的初学者也适合那些已经用过库但不知道为什么有效、调参全靠感觉的工程师。我会把原理、公式推导思路、代码实现以及实际调参踩坑的经验全部摊开来讲保证你读完能自己从零写出一版能跑的卡尔曼滤波器并且知道怎么把它用到自己的项目里。1. 卡尔曼滤波到底在解决什么问题1.1 一个传感器的数据为什么不够用先设一个场景你要测量一个东西的速度和位置手里只有一个传感器它每隔一段时间给你返回一个读数。你很快会发现两件事第一读数有噪声静止的时候它也会上下跳第二它有一定的滞后性或者漂移长期下来的累积误差会让人抓狂。那如果再加一个传感器呢比如用GPS测位置用IMU测加速度两者结合。这时候新问题出现了两个传感器数据不一致怎么办该信谁其实谁也做不到完全信任GPS有标准差几米的随机误差IMU有随温度、时间变化的漂移和偏置。你需要的不是一个“更准的传感器”而是一个能把多个不完美的信息源按各自的置信度加权融合的机制。这就是卡尔曼滤波的定位它不试图消除噪声而是在已知噪声统计特性的前提下给出一个统计意义下最优的状态估计。注意“统计意义下最优”这几个字它需要满足高斯噪声、线性系统这两个前提但这不影响它成为工程界最广泛使用的状态估计算法之一。1.2 预测和修正一个循环往复的闭环思想卡尔曼滤波的思想就6个字预测、观测、修正。把系统的状态变化看作一个动态过程在每一个时间步里先用系统模型比如牛顿运动学方程预测下一时刻的状态再把传感器观测值和预测值做一次加权融合得到一个修正后的状态估计。我们用一个生活化的例子来理解。假设你在追一辆车你手里有个望远镜能隐约看到车的大致位置同时你心里知道那辆车的运动规律比如不会瞬间瞬移。现在望远镜报告“车在100米处”但你根据上一秒的估计和车速推算它应该更可能在120米处。这时候你该怎么办两种都不全信而是给两个数分配权重位置越可信权重越大。卡尔曼滤波做的就是这件事预测值有预测协方差观测值有观测噪声协方差谁的不确定性小谁在融合中占的比重大。这个比重就是卡尔曼增益。整个过程是递推的每一轮只需要上一轮的状态和协方差不需要回溯历史数据。这点很关键意味着它可以写成实时、轻量的迭代程序这也是它能跑在单片机、嵌入式设备上的原因。你不需要存一堆历史数据只在内存里维护一个均值向量和一个协方差矩阵就能持续输出估计计算量非常小。2. 卡尔曼滤波的数学原理拆解2.1 五个公式其实只讲了两件事网上一搜卡尔曼滤波满屏都是五个公式。新手一看矩阵符号就头皮发麻但拆开看它们只是把前面说的“预测”和“修正”两步写成了严格的数学形式。预测阶段是两个公式状态预测( \hat{x}{k|k-1} F_k \hat{x}{k-1|k-1} B_k u_k )协方差预测( P_{k|k-1} F_k P_{k-1|k-1} F_k^T Q_k )它们的意思很直白上一时刻的状态乘以状态转移矩阵F再加上控制量输入得到当前时刻的状态预测同时上一时刻的协方差经过F的传递再加上过程噪声Q得到预测状态的不确定性。修正阶段是三个公式卡尔曼增益( K_k P_{k|k-1} H_k^T (H_k P_{k|k-1} H_k^T R_k)^{-1} )状态更新( \hat{x}{k|k} \hat{x}{k|k-1} K_k (z_k - H_k \hat{x}_{k|k-1}) )协方差更新( P_{k|k} (I - K_k H_k) P_{k|k-1} )修正阶段做的事情是算出预测协方差与观测噪声的比值得到增益K然后用K乘以“观测值和预测值之间的差异”这个差异叫新息或残差把它作为修正量加到预测状态上。注意如果观测噪声R特别大K会趋近于0表示你信预测多一些如果预测协方差P特别大K会趋近于某个上限表示你信观测多一些。这五个公式看着对称实际非常好记核心就是“预测产生先验修正得到后验”。一旦你理解了这个结构后面看扩展卡尔曼滤波、无迹卡尔曼滤波都是在改这两个阶段的模型形式本质骨架不变。2.2 状态向量、协方差矩阵和噪声参数的含义很多教程直接甩出状态向量、状态转移矩阵、测量矩阵、协方差但从来不解释每个量在工程里对应什么。我用自己的话重新讲一遍。状态向量 ( x ) 就是你想估计的物理量集合。比如一维运动系统状态可以写成 ( [位置, 速度]^T )再比如姿态解算状态可能是四元数或者欧拉角。选状态变量是你建模的第一步也是最重要的一步选少了估计不出来选多了计算浪费还容易引入不可观的量。协方差矩阵 ( P ) 表征你对当前状态估计的置信程度。对角线元素是各状态变量的方差非对角线元素是不同状态变量之间的相关性。比如位置和速度估计是相关的位置推得快速度也必须跟着大否则位置预测就不合理。这个相关性信息在卡尔曼滤波里会被充分利用所以你既能看到位置被修正也能看到速度被“间接”修正这是它比单独滤波每个变量高明的地方。过程噪声协方差矩阵 ( Q ) 描述的是系统模型本身的不确定性。它不来自传感器而来自你“对物理模型的信任程度”。比如你假设车辆匀速运动但它可能在加速那这个没建模的加速度就属于过程噪声需要调大Q。观测噪声协方差矩阵 ( R ) 则来自传感器一般用标定、大量静态测量的方差来估计。Q和R是卡尔曼滤波里两个最重要的“可调旋钮”它们之间的相对大小直接决定滤波器是更平滑信任模型还是更灵敏信任观测。调参困难的根源往往在于Q和R给得不准。2.3 卡尔曼增益为什么是个“比值”而不是固定常数很多入门者容易有个误区觉得卡尔曼增益像一个PID参数固定设一个值就行。但实际上卡尔曼增益每一时刻都会自动计算它本质上是预测不确定性和观测不确定性的一个比值。看增益公式的结构分子是 ( P_{k|k-1} H^T )分母是 ( H P_{k|k-1} H^T R )。如果过程噪声Q变大预测协方差P只能跟着变大增益K就会变大滤波器更愿意跟随观测如果观测噪声R变大分母变大增益K变小滤波器更相信自己的预测。你不妨把它理解成“自动调节的信任开关”——不需要你手动决定信谁它按照当前两个不确定性的相对大小来分配信度。还有一点容易被忽略在标准线性卡尔曼滤波里只要Q、R、F、H固定P矩阵的收敛轨迹其实是预先决定的K也会收敛到一个稳态值。也就是说如果你长时间在线运行滤波器会进入稳态增益基本不变。这个时候强行改成固定增益相当于实现了一个稳态卡尔曼滤波器即Wiener滤波器的一种工程形态在计算资源受限的场合是可行的近似。但在动态性强、噪声非平稳的场合让K自适应变化是更稳妥的。3. 从零手写一版卡尔曼滤波3.1 先写一维版本位置估计的完整实现我们先从最简单的一维情况入手估计一个恒定位置的物体传感器直接测量它的位置。这个情况下状态是一个标量F1H1B0P、Q、R也都是标量公式瞬间退化得很清晰。假设我们要对一个温度传感器做滤波初始位置温度猜测为25度初始协方差P为1过程噪声Q为0.001温度本身很稳定观测噪声R为0.25传感器标准差0.5度。每来一个新的观测就执行一次预测加修正import numpy as np import matplotlib.pyplot as plt # 初始状态 x np.array([25.0]) # 初始温度估计 P np.array([1.0]) # 初始协方差 Q np.array([0.001]) # 过程噪声 R np.array([0.25]) # 观测噪声 F np.array([1.0]) # 状态转移 H np.array([1.0]) # 观测矩阵 measurements [25.5, 25.8, 24.9, 25.3, 26.1, 25.7, 25.4, 26.0] estimates [] for z in measurements: # 预测 x_prior F x P_prior F P F.T Q # 计算增益 K P_prior H.T / (H P_prior H.T R) # 修正 x x_prior K * (z - H x_prior) P (1 - K * H) * P_prior estimates.append(x[0]) print(f观测{z:.2f}, 增益{K[0]:.3f}, 估计{x[0]:.3f})第一次迭代你会看到增益K是0.8左右说明滤波器非常信任观测因为初始P大。几次迭代后K会迅速掉到一个很小的值滤波器趋于稳定后续估计值基本平滑。这就是一个完整可跑的卡尔曼滤波代码里没有花哨的东西核心就是五个公式的复制粘贴。能在纸上把这一维例子手推一遍对后面理解矩阵版意义重大。有人会问温度这么稳定的系统我用一个滑动平均不就行了确实可以但在动态变化系统中滑动平均的滞后会很明显卡尔曼滤波能根据模型预测来补偿滞后。比如下面要讲的运动目标估计滑动平均追不上一个变速目标但卡尔曼滤波做得到。3.2 多维版本用Python实现位置-速度联合估计一维只是热身工程里更多是联合估计多个状态变量。最常见的例子就是匀加速运动模型状态量是位置p和速度v控制量是加速度a传感器只测位置。系统建模如下状态向量 ( x [p, v]^T )状态转移矩阵 ( F [[1, dt], [0, 1]] )其中dt是时间步长控制矩阵 ( B [[0.5 dt^2], [dt]] )观测矩阵 ( H [1, 0] )只观测位置过程噪声协方差 ( Q q * [[dt^4/4, dt^3/2], [dt^3/2, dt^2]] )这是从“加速度噪声”模型推导出来的连续时间离散化结果这段推导很多教材会跳过但它是Q矩阵怎么来的一个经典样例。假设加速度的噪声强度是q类似加速度方差经过位置、速度积分后位置噪声贡献是 ( q dt^4 / 4 )速度噪声贡献是 ( q dt^2 )协方差项是 ( q dt^3 / 2 )。我建议直接把这个形式记住它比你随便填一个对角矩阵合理得多。import numpy as np import matplotlib.pyplot as plt dt 0.1 F np.array([[1.0, dt], [0.0, 1.0]]) B np.array([[0.5 * dt**2], [dt]]) H np.array([[1.0, 0.0]]) q 0.02 Q q * np.array([[dt**4/4, dt**3/2], [dt**3/2, dt**2]]) R np.array([[0.1]]) x np.array([[0.0], [1.0]]) P np.eye(2) * 1. true_pos [] measurements [] for i in range(100): true_v 1.0 0.05 * i true_p 0.5 * 0.05 * i**2 1.0 * i z true_p np.random.randn() * np.sqrt(R[0,0]) true_pos.append(true_p) measurements.append(z) estimates [] for z in measurements: # 预测 x_prior F x P_prior F P F.T Q # 增益 S H P_prior H.T R K P_prior H.T np.linalg.inv(S) # 修正 x x_prior K (z - H x_prior) P (np.eye(2) - K H) P_prior estimates.append(x[0,0]) plt.plot(true_pos, labelTrue) plt.plot(measurements, labelMeasurements, alpha0.5) plt.plot(estimates, labelKF Estimate) plt.legend() plt.show()跑完你会发现两个特点第一滤波后的位置曲线明显比原始观测平滑第二即便只用位置观测估计的速度也能跟上真实速度的变化。这就是状态耦合的好处——位置修正会通过协方差P的非对角项传播到速度估计上让你“免费”获得一个速度估计。很多实际项目里卡尔曼滤波不只用来滤噪声更主要用来做“状态观测器”从低成本传感器里提取出不便直接测量的物理量。3.3 参数整定Q和R到底怎么调这一块是实战里最容易被忽略也最折磨人的。网上教程最喜欢说“Q和R根据噪声特性来定”但没人告诉你当传感器噪声统计不知道的时候该怎么确定。我自己的调参套路是四步走第一步确定R。这部分相对容易。把传感器放到静态场景采集几百上千个样本计算方差这个方差就是R的对角线元素。比如说IMU静止时加速度计输出的方差是0.01那你R初始值就按这个来。这里要提醒一句实际数据的噪声往往不是纯高斯可能有粗差、量化误差、慢变漂移所以用标定方差当R只是第一版后面还要微调。第二步确定Q。这部分难。因为Q描述的是“你模型没建模的那些东西”。以匀加速模型为例如果目标真实轨迹里有转弯、急加速这个模型就没法完全跟上Q需要设得足够大让滤波器保持对观测的敏感性。一个简单有效的经验是QQ的数值级先设成R的千分之一到百分之一看滤波曲线的平滑程度再从小到大扫描。第三步观察状态估计的时序曲线。如果曲线非常平滑但滞后明显说明Q太小或者R太大滤波器过信模型如果曲线跟着噪声狂跳说明Q太大或者R太小滤波器过信观测。理想状态是滤波曲线要平顺但在目标发生真实机动时有足够的响应速度跟上。第四步微调协方差的耦合项。如果状态变量之间耦合建模得准比如位置-速度的Q非对角项不是0你往往能获得更好的动态响应。注意调参没有“一键最优”。它本质是在“平滑”和“响应”之间找折中而折中的位置取决于你的下游任务如果下游是控制你更在意延迟小如果下游是显示与记录你更在意平滑。4. 实操过程与核心环节实现4.1 无人机高度估计融合气压计和加速度计把卡尔曼滤波用在无人机高度估计上是经典场景它还是理解“多传感器融合”的最佳入门。问题设定是这样的无人机上有气压计能直接测高度但气压计噪声大、响应慢还有加速度计输出纵向加速度噪声相对小但加速度积分一次才能得到速度再积一次才得到高度双重积分之后漂移极其可怕。单纯用任何一个都不行但融合起来问题就迎刃而解。状态向量还是位置-速度模型只是输入量变成了加速度计的测量值。我们把加速度作为控制量 ( u )同时给它的方差映射到Q里。这里的建模很关键加速度计测量值不是完美的控制量它有偏置和噪声所以要把控制输入的噪声折算到过程噪声里。实现上有几个细节值得注意。第一气压计输出海拔高度和无人机相对起飞点的高度差一个偏移量需要做零点校准。第二气压计受气流、螺旋桨下洗气流干扰明显悬停时也可能看到几十厘米的跳动这是正常的。第三加速度计在剧烈振动下会有高频噪声可以在前端做个低通滤波把高频分量滤掉一部分再喂给卡尔曼这样能避免高频振动通过控制输入混进状态估计。整个系统跑起来的效果是短时间尺度上高度估计主要靠加速度计积分响应快、细节好长时间尺度上加速度积分的漂移会被气压计“锚定”住不让你飘走。这种组合正是惯性导航和绝对定位传感器融合的基本范式GPSIMU组合导航也是同一个套路。4.2 目标跟踪卡尔曼滤波如何站稳主力位置在目标跟踪领域卡尔曼滤波是经典的“单目标”主力。拿视频监控里的行人跟踪来说每一帧检测器会给出目标的边界框位置但检测可能漏检、跳变、有噪声。卡尔曼滤波在帧与帧之间预测目标位置再和下一帧检测结果匹配用匹配上的检测做修正这样跟踪曲线会非常平滑。实际工程里卡尔曼滤波往往和匈牙利匹配算法配合使用。卡尔曼负责“预测平滑”匈牙利负责“多目标之间的匹配关联”。每一帧流程是用卡尔曼预测每个已有目标在当前帧的位置然后用IOU或外观特征计算预测框和检测框的代价矩阵匈牙利算法完成一一匹配。匹配上的目标用检测框修正状态没匹配上的目标可以认为是新出现的目标或已经离开的目标。这个框架在ByteTrack、DeepSORT等经典多目标跟踪算法里都能看到可见卡尔曼滤波不是过时算法它依然是现代跟踪系统里被反复验证的基础组件。用卡尔曼做目标跟踪的常见坑是检测框噪声不满足高斯分布。检测器偶尔会给出一个偏离很大的框也就是所谓的“粗差”。标准卡尔曼对粗差非常敏感一个异常检测值就能把状态拉飞。解决办法有两类一类是增加异常检测逻辑新息的马氏距离超过阈值就丢弃不参与状态更新另一类是改用鲁棒滤波或Student-t分布建模测量噪声但工程里用第一类更多简单可靠。4.3 卡尔曼滤波与惯性导航IMU融合的核心骨架搜卡尔曼滤波与惯性导航原本是很多面试和项目里绕不开的点。惯性导航本身不依赖外部信号纯靠IMU做积分短时精度高但误差随时间累积而GNSS、视觉、磁力计这些绝对观测可以周期性校正它。两者融合实际上就是卡尔曼滤波最擅长的事。这里有个专业术语叫“松组合”和“紧组合”。松组合模式里INS先独立做机械编排输出位置速度姿态GNSS接收机也独立输出位置速度卡尔曼滤波把两者之差作为观测值来估计INS的误差状态。紧组合则是直接用伪距、载波相位等原始观测做融合计算量更大但抗遮挡能力强通常用于更高端的组合导航系统。工程上如果只是做产品原型松组合足够用。实现姿态解算时状态向量往往是“姿态误差角 陀螺零偏 速度误差 位置误差”这是一个经典的15维状态。观测来自GPS位置速度或视觉位姿。因为状态里显式建模了陀螺零偏卡尔曼滤波可以在线估计零偏在每次开机时不需要长时间静态对准。这一点在工程上价值极大以前用互补滤波每次上电还要保持静止几秒去估算陀螺零偏用卡尔曼滤波把零偏放进状态里开机就能很快收敛。5. 常见问题与排查技巧实录5.1 滤波结果发散先检查“可观性”新手最容易遇到的现象是前几步滤波还不错跑着跑着估计值直接飞到天上去了彻底发散。这时候先别急着调Q和R要先检查系统是否可观。可观性的直观理解是你现有的观测能不能唯一确定系统内部的每个状态变量。如果某个状态对任何观测都不产生可区分的影响那这个状态就是不可观的滤波会沿着这个方向漂移。典型的例子用GPS只测位置想估计IMU的加速度计零偏——如果系统建模里没有把加速度计零偏跟位置观测关联起来这个零偏是不可观的滤波必然发散或漂移。工程排查方法很简单把F、H矩阵列出来算一下可观性矩阵的秩看是不是满秩。如果不满秩你就得考虑加观测、改模型、或者把不可观的状态从状态向量里删掉。很多时候删掉一个不可观的状态比调一天Q都有效。5.2 滤波后曲线有锯齿Q和R的“信任失衡”如果滤波输出虽然比原始测量平滑但依然有明显的锯齿或者毛刺大概率是卡尔曼增益偏大滤波器更信任观测了。这时候的策略是适当增大R或者减小Q让增益降下来。反过来如果滤波曲线“太完美”了完美到像一个被大幅滞后过的数据那就说明增益太小了滤波器几乎没有在用观测信息。这时减小R或者增大Q让滤波器的响应快起来。这里有个小技巧可以同时画“新息序列”观测和预测的差。如果新息均值不是0、有周期性或明显趋势说明系统模型有偏需要检查F、B建模是否准确如果新息白噪声但方差大于理论值说明R给小了或者Q给大了如果新息方差小于理论值反过来了。5.3 粗差和异常值如何让卡尔曼滤波更“抗造”前面提过标准卡尔曼假设观测噪声是高斯分布但真实数据里经常有非高斯的粗差。一个异常值会把估计值拉飞然后要花很长时间才能拉回来。解决办法是给滤波加“保护逻辑”在实践中至少有三种方案。最简单的是新息门限法每次观测进来先算新息的标准差如果新息的马氏距离超过3倍理论标准差就认为这次观测是异常值跳过更新。这个实现简单实际效果也好但对连续多帧异常时要小心因为跳过更新会让P一直增长反而更容易被下一帧带偏所以要对“跳过次数”做上限处理。更稳妥一点的是协方差膨胀法检测到异常时不丢弃观测而是把R临时放大几十倍再拿去更新让它对状态的“拉扯力”变小但不完全失效。这在高动态场景里更安全。再高级一些就是做成自适应卡尔曼滤波在线估计R的缩放因子不过实现成本较高小项目不建议一上来就上自适应先把简单的门限法用好。5.4 数值问题P矩阵变得不对称怎么办在强跟踪、长时间运行、计算机数值精度有限的情况下P矩阵可能因为计算截断误差而变得不对称甚至出现负的特征值这会导致滤波发散。最直接的解决办法是每次更新后做一次对称化处理也就是把P强制变成 ( (P P^T) / 2 )。另一个更隐蔽的问题是P矩阵的条件数太大求逆的时候数值不稳定。可以尝试用QR分解代替直接求逆或者使用平方根卡尔曼滤波。说实话现在的大部分场景用双精度浮点已经足够出现数值问题多半是模型给错了先检查建模再考虑换算法。6. 卡尔曼滤波的变体与选型参考6.1 EKF非线性系统中的泰勒展开线性化标准卡尔曼滤波严格限制在线性系统和高斯噪声。现实世界里系统几乎都是非线性的比如无人机姿态解算里的四元数更新、目标跟踪里的极坐标观测都存在明显的非线性关系。扩展卡尔曼滤波EKF的思路很直接在每个时间步把非线性函数在当前状态估计点附近做一阶泰勒展开用雅可比矩阵代替F和H然后再套用标准卡尔曼的公式。优点是实现简单延续了原算法的结构缺点是一阶近似在强非线性下误差大而且雅可比矩阵推导麻烦容易出错。在我的项目经验里EKF适合那些非线性程度不太强、且你对模型比较有把握的场景。比如车辆运动模型、惯性导航误差模型都有成熟的雅可比推导可以参考。但如果你用EKF发现滤波很不稳定大概率不是参数问题而是线性化误差太大算法框架不适合当前问题。6.2 UKF和粒子滤波什么时候需要更“高级”的滤波无迹卡尔曼滤波UKF用一组精心选择的Sigma点来传播概率分布不需要计算雅可比矩阵对强非线性系统的估计精度通常优于EKF。它和EKF的计算量差距没有想象中那么大所以在许多新项目中UKF已经逐渐取代EKF成为默认选项。粒子滤波则完全不假设高斯分布它用一堆带权重的粒子来近似任意分布适合多峰、非高斯、严重非线性问题但计算量大粒子数量随状态维度增加呈爆炸式增长工程里慎用。选型建议可以直接记结论系统线性用标准KF弱非线性且不想改太多代码用EKF强非线性、代价允许稍微大一点用UKF多峰分布、非高斯噪声、且维度不高才考虑粒子滤波。不要一上来就追求“高级”很多实际问题里标准KF加一个良好的建模就已经表现得很好了。6.3 实际项目中的选型误区我在社区里经常看到有人一上来就问“我用卡尔曼还是粒子滤波”其实这个问题问错了。真正要问的是“我的系统模型可靠吗我的观测模型可靠吗我的噪声统计能不能拿到”如果系统模型和观测模型一塌糊涂用粒子滤波也只能得到“精确的错答案”。先把模型的物理意义搞清楚把噪声统计量估计出来再回头看算法选型你会发现大多数项目用卡尔曼或者EKF就能解决。另一个常见误区是以为卡尔曼滤波能“消除”误差。它只能融合信息降低不确定度不能突破观测或模型本身的信息极限。如果你的传感器本身信息量不足卡尔曼滤波是变不出高精度结果的。7. 最后分享几点实操体会写到这里核心的原理、代码和坑都讲完了。最后说点我自己的体会不是什么结论纯粹是经验。我做任何涉及滤波的项目第一件做的事永远是“画数据曲线”原始数据长什么样、噪声大概什么量级、有没有粗差直接决定了我后面怎么建模、怎么设Q和R。数据都没看清就急着套卡尔曼公式是新手最典型的失败路径。第二个体会是滤波器的“好”没有绝对标准。同一个卡尔曼参数在无人机悬停场景里看着很完美一到急加速机动就露馅。你必须先明确下游任务要什么——是要低延迟还是低波动是要快速收敛还是稳态精度目标函数不同最优参数完全不一样。第三个体会是不要迷信公式和库。你可以用现成的库比如pykalman、filterpy快速跑通但一定要能手写出那五个公式。因为一旦出问题你只有理解了每一步在干嘛才能快速定位是模型错、参数错还是逻辑错。很多看起来“玄学”的滤波bug最后归根到底都是建模问题调试的根本还是回到那张状态方程和观测方程上去。卡尔曼滤波是一个越用越觉得有意思的算法它不复杂但要把它用得顺手、用得稳背后是对系统建模的理解和对噪声统计的直觉。希望这篇东西能帮你跳过一些我以前踩过的坑直接把精力花在真正重要的事情上。
返回列表