简介这是一份Fortran语言编写的卡尔曼滤波完整程序包面向需要掌握状态估计算法及数值实现的工程技术人员与科研学习者适用导航定位、信号处理、过程控制等动态系统领域。压缩包共38个文件约959KB包含f90源程序、dat数据文件、mod模块、obj目标文件、exe可执行文件及sln工程配置等便于直接编译运行与对照学习。目前已有678人学习下载。程序完整覆盖系统动态模型、测量模型、初始化、卡尔曼增益计算及预测-更新迭代循环并通过多个子程序实现随机数据生成、矩阵求逆、迹运算和数据读取配套示例数据文件和readme说明可清晰复现整个滤波流程帮助读者深入理解算法原理后快速迁移至实际项目。1. 卡尔曼滤波程序fortran这套老代码今天还在解决什么问题卡尔曼滤波程序fortran这个词对很多从业者来说是两样东西一批跑了二三十年的工业代码以及一套依然值得亲手写一遍的数值算法。我最早接触它是在一个传感器标定模块里那套老代码没有一行注释但输出稳定得让我印象深刻——后来我用其他语言重写反而在数值精度和内存布局上折腾了很久。这篇笔记想讲清楚的是这个标题到底在解决什么问题、五个方程怎么落成 Fortran 数组、最小程序怎么写以及那些让人半夜排查的坑。适合正在做控制、定位或传感器数据处理又需要在老代码库或实时环境里动手的开发者。2. 卡尔曼滤波的五个方程拆开看哪些必须用矩阵哪些能用标量2.1 状态预测和协方差传播一分钟理解卡尔曼滤波在算什么用一个场景把直觉建立起来你有一辆车已知它上一秒的位置和速度要猜它这一秒的位置。你能给出一个估计值但这个估计有多可信取决于模型的粗糙程度和观测的噪声水平。卡尔曼滤波做的是把“估计值”和“我不确定到哪种程度”两样东西同时往前推再用观测去修正修正幅度取决于你到底更信任模型还是更信任观测。这里的“不确定程度”就是协方差矩阵它决定增益的大小而增益决定了滤波器的性格。对应到 Fortran 程序上这个循环就是五个方程的顺序执行预测状态、预测协方差、计算增益、更新状态、更新协方差。一维情况下它们都是标量运算写成普通乘除法即可一旦状态向量超过一维矩阵乘法和转置就不可避免这时 Fortran 提供的 matmul 和 transpose 直接映射公式比手工嵌套循环更不易出错编译优化也更好。协方差矩阵在这套循环里扮演的角色经常被低估。状态估计值只是“当前最可能的数值”协方差才决定“这个数值可信到什么程度”。增益 K 本质上是一个比值估计的不确定性除以估计的不确定性加观测的不确定性。这个比例让滤波器天然地在模型可靠时少听观测、在模型不可靠时多听观测。理解这条主线后面调 Q 和 R 就是在调两个不确定性的比值而不是漫无目的地试参数。还要注意五个方程的执行顺序不能乱。先 predict 再 update 才是滤波反过来先更新再预测语义就变成了平滑结果完全不一样。很多新手把模块写对了却在主程序里把两个子程序的调用顺序搞反曲线也看不出大问题但相位和方差行为都不对。2.2 Fortran 里矩阵运算的原生表达matmul、transpose 与数组声明Fortran 90 之后写矩阵运算是很顺手的。下面这段演示一个二维状态向量上的预测步状态转移矩阵为 A过程噪声协方差为 Qreal, dimension(2) :: x real, dimension(2,2) :: a, p, p_new, q x matmul(a, x) p_new matmul(matmul(a, p), transpose(a)) q代码逻辑说明第一句把上一时刻状态乘上转移矩阵得到先验估计第二句等价于 A·P·Aᵀ Q这里连续嵌套两次 matmul内层先算 A·P再乘 transpose(a)。注意 Fortran 里两个数组相加是对应元素相加而矩阵乘法必须用 matmul这两者混用是新手最容易翻车的地方。参数说明A 是 n×n 的状态转移矩阵P 是 n×n 的协方差矩阵Q 必须是对称半正定矩阵。把 Q 初始化成对角阵是最常见的做法对角元素代表各状态分量的过程噪声方差。如果程序中 Q 被填成了非对称阵滤波大概率跑到一半协方差矩阵就会越走越歪这一点在第 5 章专门讲。嵌套写法还有一个好处代码和公式一一对应review 的人一眼能看出每一行在算什么。写成 p matmul(a, matmul(p, transpose(a))) q 也完全等价编译器优化后没有差别。但不要写成 matmul(matmul(p, a), transpose(a))维度对不上是一回事语义上顺序也错了。Fortran 数组默认列主序存储意味着 a(2,1) 在内存里紧跟在 a(1,2) 后面。对 matmul 和 transpose 来说编译器会处理好布局你不需要手动重排但如果你手写循环累加矩阵乘就非常容易把索引写反。另一个实际影响是把二维数组作为参数传给子程序时只有显式给出维度的接口才不会出错很多老代码在传递二维数组时频繁踩坑后面第 5 章会专门展开。2.3 增益 K 的计算为什么求逆是唯一的除法点卡尔曼滤波整个算法里唯一需要做“除法”的地方是计算增益而多维时这个除法变成矩阵求逆。一维情况增益是标量除法零维问题了二维以上就要对创新协方差矩阵求逆。对规模很小的系统2×2 或 3×3我一般直接写一个手求逆子程序避免引入外部依赖也方便单步调试系统更大就换数值线性代数库那属于另一个话题。下面是一个 2×2 矩阵求逆的 Fortran 实现配合增益计算刚好够用subroutine mat_inv2(a, inv_a) real, intent(in) :: a(2,2) real, intent(out) :: inv_a(2,2) real :: det det a(1,1)*a(2,2) - a(1,2)*a(2,1) if (abs(det) 1.0e-12) then write(*,*) warning: matrix near singular, det, det inv_a 0.0 return end if inv_a(1,1) a(2,2)/det inv_a(1,2) -a(1,2)/det inv_a(2,1) -a(2,1)/det inv_a(2,2) a(1,1)/det end subroutine mat_inv2逻辑说明二阶矩阵的逆直接由行列式倒数和元素交换、变号组成inv_a(1,1) 对应原矩阵的 a(2,2)inv_a(1,2) 是负的 a(1,2)以此类推。临界值 1.0e-12 是按典型浮点量级选的如果你的数据整体偏大或偏小这个阈值需要跟着调整。参数说明当 |det| 过小说明矩阵接近奇异此时增益计算会爆出异常大的值让滤波器瞬间跳飞。阈值太小起不到保护作用太大又会误伤正常更新。我一般在调通算法前会把阈值写成可打印的变量跑一遍真实数据后看它有没有被频繁触发再决定放大还是缩小。什么时候不要手写求逆当状态维度超过 3二阶公式不再适用需要通用高斯消元或调用数值线性代数库。很多现代编译器的运行时已经挂了 BLAS 和 LAPACK链接时加上对应库即可。调用库函数的好处是稳定和快坏处是你必须把数组声明成库期望的布局列主序问题在接口处暴露得最彻底。老代码里经常看到为绕开库函数而写的一长串手算逆矩阵函数那通常是为了可移植性付出的代价新项目不建议模仿。3. 从零写一个卡尔曼滤波程序fortran最小实现与参数设定3.1 最小可运行的一维标量卡尔曼滤波模块真实工程里第一次动手我建议从一维开始一个变量要估计、一个观测通道、一个标量状态转移。这东西能跑通理解就立住了再升维只是把标量换成数组。下面是一个完整的一维卡尔曼滤波模块保存为 kf_module.f90module kf_module implicit none type :: kf_state real :: x ! 当前估计值 real :: p ! 当前估计方差 real :: f ! 状态转移系数 real :: h ! 观测系数 real :: q ! 过程噪声方差 real :: r ! 观测噪声方差 end type kf_state contains subroutine kf_init(st, x0, p0, f, h, q, r) type(kf_state), intent(inout) :: st real, intent(in) :: x0, p0, f, h, q, r st%x x0 st%p p0 st%f f st%h h st%q q st%r r end subroutine kf_init subroutine kf_predict(st) type(kf_state), intent(inout) :: st st%x st%f * st%x st%p st%f * st%p * st%f st%q end subroutine kf_predict subroutine kf_update(st, z) type(kf_state), intent(inout) :: st real, intent(in) :: z real :: k, innovation innovation z - st%h * st%x k st%p * st%h / (st%h * st%p * st%h st%r) st%x st%x k * innovation st%p (1.0 - k * st%h) * st%p end subroutine kf_update end module kf_module逻辑说明kf_init 负责把初始状态和四个参数一次性塞进派生类型里kf_predict 完成状态与方差的前推方差更新对应 σₜ₊ f²·σₜ qkf_update 先算新息再算增益 k用 k 修正状态并收缩方差。整个模块没有依赖外部库直接可以在任何支持 Fortran 90 的编译器上编译。参数说明f 是状态转移系数匀速直线运动里 f1如果状态定义成“位置加速度”而不是纯位置f 就不再是标量而是矩阵见 3.2。h 是观测系数直接量测时 h1。q 和 r 的比值决定滤波器是更信任模型还是更信任观测q/r 偏大增益偏大滤波结果更跟手但噪声也大q/r 偏小输出平滑但滞后明显。p0 是初始估计方差给大比如 10.0会让滤波器在头几步快速收敛到观测附近给小比如 0.1则前几十步反应迟钝。实际接入数据前我建议把所有的 real 换成 real(8)能少踩一半数值坑。这个模块在主程序里的用法很简单先 call kf_init(st, 0.0, 10.0, 1.0, 1.0, 0.001, 0.04)然后每来一个观测先 predict 再 update。再次提醒顺序反了就不是滤波而是平滑。3.2 升级到两维状态向量位置与速度联合估计实际场景里很少只估计一个标量。最常见的起步模型是“位置加速度”两个状态用位置量测。选这个模型的原因是它覆盖了大部分匀速或缓变运动对象状态里带上速度滤波器就能在观测短暂缺失时靠预测顶住这是标量模型做不到的。此时状态转移矩阵为 A [[1, dt], [0, 1]]观测矩阵 H [1, 0]。预测步和更新步的核心代码如下subroutine predict2(x, p, a, q, dt) real, dimension(2), intent(inout) :: x real, dimension(2,2), intent(inout) :: p real, dimension(2,2), intent(inout) :: a real, dimension(2,2), intent(in) :: q real, intent(in) :: dt a(1,1) 1.0; a(1,2) dt a(2,1) 0.0; a(2,2) 1.0 x matmul(a, x) p matmul(matmul(a, p), transpose(a)) q end subroutine predict2 subroutine update2(x, p, z, h, r) real, dimension(2), intent(inout) :: x real, dimension(2,2), intent(inout) :: p real, intent(in) :: z, r real, dimension(1,2), intent(in) :: h real :: s, innovation real, dimension(2) :: k, hp innovation z - (h(1,1)*x(1) h(1,2)*x(2)) s h(1,1)*(p(1,1)*h(1,1) p(1,2)*h(1,2)) h(1,2)*(p(2,1)*h(1,1) p(2,2)*h(1,2)) r k(1) (p(1,1)*h(1,1) p(1,2)*h(1,2)) / s k(2) (p(2,1)*h(1,1) p(2,2)*h(1,2)) / s x x k * innovation hp(1) h(1,1)*p(1,1) h(1,2)*p(2,1) hp(2) h(1,1)*p(1,2) h(1,2)*p(2,2) p(1,1) p(1,1) - k(1)*hp(1) p(1,2) p(1,2) - k(1)*hp(2) p(2,1) p(2,1) - k(2)*hp(1) p(2,2) p(2,2) - k(2)*hp(2) end subroutine update2逻辑说明predict2 里 A 是常数矩阵实际项目里只在初始化时填一次即可这里为了演示才在每次预测时赋值。x matmul(a, x) 把位置向前推 dt·v速度保持不变协方差用同样的传播规律加上过程噪声 q。update2 里 s 是创新方差计算方式是 H·P·Hᵀ R 的标量展开增益 k 是二维向量公式是 P·Hᵀ/s。最后四句用外积形式完成 P 的更新等价于 P P - K·H·P只是绕开了定义外积函数的麻烦。参数说明Q 的对角元素分别是位置和速度的过程噪声方差一般位置取小、速度取大R 对应观测噪声方差按传感器标称精度填。Q 和 R 的比值一旦差出三个数量级以上滤波器的行为会非常极端要么完全无视观测要么完全无视模型这是第 4 章调参的重点。这套代码再往上走就是通用的 n 维实现差别只在数组维度上subroutine kf_predict_n(x, p, a, q, n) integer, intent(in) :: n real, dimension(n), intent(inout) :: x real, dimension(n,n), intent(inout) :: p real, dimension(n,n), intent(in) :: a, q x matmul(a, x) p matmul(matmul(a, p), transpose(a)) q end subroutine kf_predict_n逻辑说明Fortran 90 之后支持假定形状参数子程序里用 dimension(n) 就能接收任意维度的数组。前提是主程序里显式声明了对应形状并且接口块或模块提供了显式接口否则编译器无法检查维度运行期才可能报错。3.3 代码怎么组织模块化、主程序和编译命令实际工程中滤波内核、观测模型、测试驱动应该分文件放。我常用的结构是kf_core.f90 放卡尔曼滤波模块sensor_model.f90 放噪声生成、数据读取等和传感器相关的东西test_driver.f90 只做一件事把数据喂给滤波并输出结果。这样换场景时只需改初始化参数内核代码不动。编译命令建议这样写gfortran -O2 -ffree-form -Wall kf_core.f90 sensor_model.f90 test_driver.f90 -o test_kf参数说明-ffree-form 是告诉编译器源代码使用自由格式如果你的源文件是 .f 后缀的固定格式去掉这个选项即可。开发阶段建议用 -O0 -g 方便调试器定位正式跑数据时再用 -O2。-Wall 打开所有警告隐式类型声明、未初始化变量都会在编译时暴露。模块化最大的好处是测试驱动可以随时换成真实数据流只要保证调用接口不变滤波内核一行都不用动。老代码里最常见的坏味道是滤波和 IO 写在一起导致想单独测滤波器时还得先准备一个文件。4. 让卡尔曼滤波程序fortran跑在真实数据上模拟验证与调参路线4.1 造一份带噪声的模拟数据让滤波器先过一遍玩具用例动手调参前先构造一份已知答案的模拟数据。常见做法是生成一条匀速直线轨迹叠加高斯噪声然后把带噪数据喂给滤波器对比滤波结果与真实轨迹。下面这个子程序用 Box-Muller 变换生成高斯随机数避免依赖编译器自带的随机分布subroutine gauss_rand(mean, std, val) real, intent(in) :: mean, std real, intent(out) :: val real :: u1, u2 call random_number(u1) call random_number(u2) val mean std * sqrt(-2.0*log(max(u1, 1.0e-10))) * cos(2.0*3.14159265*u2) end subroutine gauss_rand逻辑说明random_number 产生 [0,1) 均匀随机数max(u1, 1.0e-10) 防止取对数时碰到 0Box-Muller 用两个均匀分布合成一个标准正态分布。真实值按匀速推进观测值在真实值上叠加高斯噪声。参数说明std 按传感器标称精度设置。这里最关键的是模拟时的噪声水平要和滤波器里填的 R 一致。如果模拟用 std0.7滤波器里却填 R0.01相当于人为制造“观测噪声远小于实际”的假象滤波结果看起来收敛极快真实数据一进来立刻发散。模拟和 R 不一致是新手最常踩的坑没有之一。生成数据后建议写成两列文件真实值和观测值。后面评估滤波质量时需要真实值做基准。一个完整的测试驱动长这样program test_driver use kf_module implicit none type(kf_state) :: st real :: true_x, obs integer :: i, n n 500 true_x 0.0 call kf_init(st, 0.0, 10.0, 1.0, 1.0, 0.001, 0.04) open(10, filekf_out.txt, statusreplace) do i 1, n true_x true_x 0.02 call gauss_rand(true_x, 0.2, obs) call kf_predict(st) call kf_update(st, obs) write(10, *) true_x, obs, st%x end do close(10) end program test_driver逻辑说明这里模拟的观测噪声标准差是 0.2对应方差 0.04正好等于 R真实信号每步只漂移 0.02模型误差很小Q 也不用给太大。循环里先 predict 再 update输出三列真实值、观测值、滤波估计值。4.2 输出结果和评估指标RMSE、滞后和收敛时间滤波效果不能靠肉眼“看起来贴合”判断。我一般算三个指标滤波后与真实值的均方根误差 RMSE、滤波输出的滞后步数、以及初始收敛所需步数。RMSE 在 Fortran 里只是循环求和开根号rmse 0.0 do i 1, n rmse rmse (x_est(i) - x_true(i))**2 end do rmse sqrt(rmse / real(n))逻辑说明x_est(i) 是滤波估计序列x_true(i) 是已知的真实轨迹这个值越小说明整体越贴近真实。滞后步数看的是滤波曲线比真实曲线慢多少拍通常用互相关或人工观察获得作为辅助判断。只看总量 RMSE 会掩盖一个问题前 20 步收敛阶段的误差可能相当大。我一般把前 1/10 的点和后面的点分开统计如果前段误差明显偏大把 p0 调大即可不需要动 Q/R。下面这张表来自另一次针对更高噪声数据的模拟实验滤波前 RMSE 约为 1.3三组配置下的滤波后量级对比很能说明问题配置过程噪声 Q观测噪声 R滤波后 RMSE滞后/噪声表现强平滑1e-50.5约 0.42曲线平滑但滞后明显平衡0.0050.5约 0.31响应适中噪声略大强跟手0.10.5约 0.38基本没有滞后残差偏大参数说明Q 从 1e-5 提到 0.1滤波器性格从“极度信任模型”变成“极度信任观测”RMSE 反而在中间取到最小值。这说明调参不是一味把 Q 调大或调小而是找一个与真实系统动态匹配的值。表格里的数字只代表某次模拟的量级具体数值随轨迹速度、采样频率和噪声水平会变但“中间最优”这个规律是稳定的。这里有个玄学味很浓的现象不同人对同一数据调的 Q/R 能差一个数量级各自结果都“能看”。原因是真实系统的过程噪声很难精确建模Q 只是把模型误差打包成一个方差。接受这一点调参就会更从容——你调的不是“正确值”而是“在滞后和噪声之间选一个可接受的折中点”。4.3 观测缺失和变步长真实接入绕不开的两个问题真实传感器很少完美等间隔、无缺失地送数据。观测缺失时常见做法是跳过更新步只执行预测步直到下一次观测到来写出来就是一个分支if (has_obs) then call kf_predict(st) call kf_update(st, obs) else call kf_predict(st) end if逻辑说明缺失观测期间只预测不更新协方差会持续膨胀恢复观测后增益自动变大滤波器会快速拉回真实值。这是卡尔曼滤波的自适应特性看到 P 变大不要急着当成 bug 去修。变步长场景则要把 dt 作为参数传进预测子程序动态填到 A 矩阵的 (1,2) 位置而不是固定在初始化时。这两段逻辑都不复杂但很多人忽略固定步长假设一旦被打破A 矩阵还按旧 dt 填位置预测就会系统性偏快或偏慢滤波效果会被无声地劣化。5. 卡尔曼滤波程序fortran避坑指南收敛、负方差和隐式声明的五类翻车现场卡尔曼滤波程序fortran的报错不太会在编译期出现更多是运行到一半才发现结果不对。下面五条是我这些年反复踩过的按症状列出来方便你直接对号入座。5.1 现象协方差矩阵对角线出现负值或矩阵不对称现象跑着跑着 p(1,1) 变成负数或 p(1,2) 不等于 p(2,1)滤波结果开始震荡甚至发散。原因最常见的是更新步协方差公式 P (I - K·H)·P 在数值上不保证对称尤其是用单精度、步长又很小时误差会累积另一个是自己手写循环时把 p(1,2) 和 p(2,1) 改得不一致。卡尔曼理论要求协方差对称且半正定数值上一旦破坏对称性增益会越算越离谱。解决一是全程用 real(8) 双精度二是每次更新后强制对称化一行代码即可p(2,1) p(1,2)如果这一步都没有用就改用 Joseph 形式的协方差更新P (I - K·H)·P·(I - K·H)ᵀ K·R·Kᵀ数值稳定性最好代价是矩阵乘法变多。系统规模变大时直接用数值线性代数库实现这个公式别手写。5.2 现象滤波发散估计值越追越远现象估计曲线刚开始正常几十步后突然甩开真实值之后再也拉不回来或者输出变成一条剧烈抖动的曲线。原因三条路径最常见。第一Q 设成 0模型又不够准误差被当成确定性的协方差不断收缩增益趋近于 0滤波器失明。第二R 设得比实际噪声小太多增益饱和滤波器把观测噪声当真值。第三初始 P 给得过大配合小 R头几步增益冲到接近 1状态被噪声主导直接跑偏。解决先把 Q 设成一个保守的小正数比如 1e-3再逐步放大同时检查新息序列——滤波正常时新息是零均值白噪声。可以把一段时间的新息打印出来看均值均值明显偏离 0 说明模型有偏要先校正模型而不是调 Q/R。5.3 现象输出平滑得过分滞后肉眼可见现象滤波曲线非常干净跟真实轨迹几乎“贴不紧”转折处慢半拍。原因Q 太小或 R 太大增益被压得很低滤波器主要靠模型外推观测只做轻微修正。这在视觉上很舒服但在定位、控制场景里会累积不可接受的位置误差。解决增大 Q 或减小 R 让增益恢复。经验起点是把 Q 设为 R 的 1/100 到 1/50 量级再按 RMSE 微调。注意调大 Q 的同时要接受输出噪声变大这是折中而不是 bug。5.4 现象编译通过、运行报错矩阵形状整不明白现象matmul 两个数组维度对不上程序运行时报 shape mismatch或者明明在别的语言里按行主序填数据转到 Fortran 后矩阵整体转置了。原因Fortran 数组是列主序声明 a(2,2) 时元素在内存里的顺序是第一列两个元素在前第二列在后。从其他语言转过来的人按行主序填常数初始化直观上“矩阵写对了”内存里实际是转置的。另一个常见原因是 matmul 的维度规则A(2,3) 乘 B(3,1) 合法A(2,3) 乘 B(2,1) 直接报错。解决养成打印 shape 的调试习惯写一个小的调试函数输出每个矩阵的维度。初始化矩阵时用 reshape 加显式次序不要依赖默认数组构造顺序。编译时加上 -fcheckall 一类的选项让越界和形状错误在开发期就暴露而不是等到数据跑了一半才崩。5.5 现象隐式类型声明把整型当实数计算结果神秘出错现象某个变量明明赋了 0.5打印出来却是 0循环里累加的结果变成整数。原因Fortran 有隐式类型规则变量名以 i、j、k、l、m、n 开头的默认是整型。如果你用 k 做增益变量赋值 k 0.5 会被截断成 0。这在老代码里几乎每次都能撞到。解决在每个程序单元开头写 implicit none一个都不放过。然后变量声明里显式写 real :: k。如果是接手老代码先全局加 implicit none再根据编译错误逐个补类型。这一步做完至少能排除一类最隐蔽的翻车原因。6. 把卡尔曼滤波程序fortran用顺手验证习惯与下一步扩展6.1 一个习惯先跑离线仿真再做在线接入我现在的固定流程是构造成分已知的模拟轨迹离线跑一版滤波把估计值、协方差、新息存成文件画图加 RMSE 检查一遍确认无误后再接实时数据。这个习惯帮我挡掉了许多接入现场才暴露的问题。另一个小技巧是把每次调参的 Q/R 和对应 RMSE 记录在一起攒几轮就能看出趋势比凭记忆调参靠谱得多。6.2 从线性到扩展卡尔曼滤波的迁移路径当状态转移或观测是非线性时比如用磁力计角度、用距离做定位线性滤波就失效了。扩展到扩展卡尔曼滤波的改动并不大把 A 换成状态转移函数的雅可比矩阵把 H 换成观测函数的雅可比矩阵其余循环结构和代码组织完全不变。我在迁移时最注意的不是公式而是雅可比矩阵的求法——数值差分求导最省事但步长选不好会引入数值噪声解析推导最可靠但每换一个场景都要重推一遍。程序完成后用同样的模拟轨迹验证如果滤波效果和线性场景相差明显先怀疑雅可比写错了而不是参数没调好。6.3 收尾写了这么多最想分享的一条经验是卡尔曼滤波程序fortran的维护者不缺公式缺的是对数值细节的敬畏。老代码里那些看似多余的对称化、显式类型声明、打印新息的口子都是前人拿真实数据换来的。我后来养成的习惯是每接一个新传感器先造数据、先离线验、先看新息再谈在线接入。希望帮到你。本文还有配套的精品资源点击获取