Fortran实现卡尔曼滤波：从矩阵公式到工程落地避坑指南

简介：这是一份用Fortran实现卡尔曼滤波算法的完整程序包，面向控制系统设计与导航定位、信号处理等领域的开发者、研究生和本科生，可帮助理解噪声环境中状态估计与预测更新机制。压缩包共38个文件，rar格式，体积约959KB；其中7个f90源文件构成核心算法，12个dat文件用于存放观测数据和仿真结果，mod、obj文件为编译模块与中间产物，另附说明文档、可执行程序和工程配置，方便快速运行和二次开发。已有678人学习下载。代码完整覆盖动态模型构建、测量模型设定、协方差初始化、卡尔曼增益计算及预测更新主循环，并配套矩阵求逆、随机数生成等实用子程序。通过示例数据可观察滤波前后状态曲线与协方差收敛过程，适合作为工程级Kalman滤波实现的参考模板。

1. 卡尔曼滤波程序（Fortran）还有人用吗：先看它在你项目里的价值

接到一个存量项目，要在老代码库里加状态估计功能，打开工程一看，主程序、数值核心、数据处理全是 Fortran 写的，这时候你要么用 C 重写一遍再对接，要么直接用 Fortran 把卡尔曼滤波程序写进去。前者工作量大、还容易引出混编问题，后者反而是最自然的选择。卡尔曼滤波本身是线性离散系统状态估计的经典方案，而 Fortran 在矩阵运算密集型任务上的性能表现至今不弱，导航、气象、航空航天、惯性测量这些领域的大量核心代码库仍然是 Fortran 写的。这篇文章就解决一件事：把卡尔曼滤波的五个标准公式落成一个能编译、能运行、能调参的 Fortran 程序，同时把 Q、R 怎么设、协方差为什么发散、混编为什么翻车这些实际问题讲透。适合刚接手 Fortran 项目、需要在老代码里加入状态估计模块的开发者。

2. 卡尔曼滤波的数学骨架：五个公式和你必须先定下来的矩阵

卡尔曼滤波不是某个库函数，而是一套递推估计规则。写 Fortran 程序之前，先把数学模型在纸面上理清：你需要定义状态向量、观测向量、四个矩阵和一个初始协方差。漏掉任何一项，程序跑起来都是玄学。

2.1 状态方程与观测方程：先学会把问题翻译成矩阵

卡尔曼滤波处理的是线性离散系统。状态方程写成：

x(k) = F · x(k-1) + B · u(k) + w(k)

其中 x(k) 是 n 维状态向量，F 是 n×n 状态转移矩阵，B 是控制输入矩阵，u(k) 是控制输入向量，w(k) 是过程噪声。观测方程写成：

z(k) = H · x(k) + v(k)

z(k) 是 m 维观测向量，H 是 m×n 观测矩阵，v(k) 是测量噪声。两个噪声都假设为零均值高斯分布，协方差分别是 Q 和 R。

这里要提醒一句：写代码前第一件事不是定义变量，而是把所有矩阵的维度写清楚。拿一维位置跟踪举例，状态选位置和速度，n=2；观测只有位置，m=1。此时 F 是 2×2，H 是 1×2，Q 是 2×2，R 是 1×1。维度对不上，后面所有 matmul 都会崩溃。

这个表里最容易轻视的是初始 P。很多人上来把 P 设成全零矩阵，结果滤波器一启动就“锁死”，后面的观测几乎不起作用。我一般把 P 设成对角阵，对角线取状态量初值不确定度的平方，比如位置不确定 10 米，P(1,1) 就取 100。

2.2 预测步两个公式：先验估计和协方差如何向前传

预测步只用模型信息，把状态和协方差往前推一步：

x_pred = F · x_est P_pred = F · P_est · F^T + Q

第一个公式好理解，就是把上一时刻的最优估计通过状态转移矩阵推到当前时刻。第二个公式要特别注意，P_pred 里必须加 Q。如果你在实现时漏掉了 Q，P 会随着迭代不断缩小，滤波器会变得“过于自信”，卡尔曼增益 K 趋近于零，后面的观测再也修正不了状态。我见过不少滤波发散案例，根因就是这里漏加 Q，或者 Q 设置得比实际模型误差小两三个数量级。

2.3 更新步三个公式：增益、修正和协方差的收敛逻辑

拿到新观测 z(k) 之后，进入更新步。三个公式按顺序执行：

K = P_pred · H^T · (H · P_pred · H^T + R)^(-1) x_est = x_pred + K · (z - H · x_pred) P_est = (I - K · H) · P_pred

K 是卡尔曼增益，它决定估计值在多大程度上信任新观测。括号里的 (z - H·x_pred) 叫新息，是实际观测与预测观测的差。P_est 公式里的 I 是单位矩阵。

注意一个实现细节：H·P_pred·H^T + R 是 m×m 矩阵。当观测维数 m 很小（比如 1 到 3）时，直接求逆没太大问题；但当 m 较大时，不要显式求逆，应该用线性方程组求解器解 K，否则数值误差会被放大，协方差容易变得不对称。这一点在第 5 章的避坑部分还会展开。

五个公式对应到 Fortran 里，核心操作就是矩阵乘法、转置、加减法和一个 m×m 矩阵的逆或线性求解。下一章我给出一个最小可用实现，不依赖任何外部库，一条 gfortran 命令就能编译运行。

3. 用Fortran从零写一个可运行的最小卡尔曼滤波：模块、子例程与主程序

我一般把卡尔曼滤波程序拆成三层：类型定义、滤波子例程、主程序。类型定义管数据封装，子例程管预测和更新，主程序负责初始化矩阵和模拟数据。这样拆的好处是后面做多目标跟踪或者嵌入其他项目时，只需要复制模块文件，改改初始化就行。

3.1 派生类型与矩阵初始化：n维状态一次性封装

用 Fortran 90 的派生类型把滤波器状态封起来，每个矩阵都声明为可分配的动态数组。要注意用 selected_real_kind 定义双精度 kind 参数 dp，而不是直接写 real(8)，后者在不同编译器上的保证性不如前者。

module kf_module implicit none integer, parameter :: dp = selected_real_kind(15, 307) type :: kf_state integer :: n = 0 ! 状态维数 integer :: m = 0 ! 观测维数 real(dp), allocatable :: F(:,:) ! 状态转移矩阵 n×n real(dp), allocatable :: H(:,:) ! 观测矩阵 m×n real(dp), allocatable :: Q(:,:) ! 过程噪声协方差 n×n real(dp), allocatable :: R(:,:) ! 测量噪声协方差 m×m real(dp), allocatable :: P(:,:) ! 误差协方差 n×n real(dp), allocatable :: x(:) ! 状态估计 n end type kf_state contains ! 内部子例程都在此 end module kf_module

dp 类型参数的作用是统一全模块的精度，避免不同子例程里单双精度混用导致隐式转换。n 和 m 记录维度，后面所有子例程都可以用它们检查数组尺寸。这个类型本身不含任何算法逻辑，属于纯数据结构，方便在其他程序里复用。

3.2 预测与更新子例程：矩阵运算和线性求解的实现

预测子例程做两个操作：状态前推和协方差前推。协方差前推需要计算 F·P·F^T，我拆成两步：先算 FP = F·P，再算 FPF = FP·F^T，避免一步写完导致中间变量过大时分配混乱。

subroutine kf_predict(kf) type(kf_state), intent(inout) :: kf real(dp), allocatable :: FP(:,:), FPF(:,:) allocate(FP(kf%n, kf%n), FPF(kf%n, kf%n)) FP = matmul(kf%F, kf%P) FPF = matmul(FP, transpose(kf%F)) kf%x = matmul(kf%F, kf%x) kf%P = FPF + kf%Q deallocate(FP, FPF) end subroutine kf_predict

这里的意图是：先用临时数组缓存中间结果，避免把 F 或 P 覆盖掉。如果你直接写 kf%P = matmul(kf%F, matmul(kf%P, transpose(kf%F)))，Fortran 的求值顺序虽然会先算右侧，但临时数组的分配和回收完全交给编译器，在大型矩阵上反而可能因为反复分配造成性能抖动，我习惯显式分配。

更新子例程稍微复杂，需要求 H·P·H^T + R 的逆。我在这里手写了一个高斯-约当消元求逆，目的是让程序不依赖 LAPACK 也能独立编译运行。注意这个选择只适合 m 很小的情况，生产环境建议换成 LAPACK 的 dgesv。

subroutine kf_update(kf, z) type(kf_state), intent(inout) :: kf real(dp), intent(in) :: z(:) real(dp), allocatable :: PHt(:,:), S(:,:), Sinv(:,:) real(dp), allocatable :: K(:,:), z_pred(:), y(:), IKH(:,:) integer :: i allocate(PHt(kf%n, kf%m), S(kf%m, kf%m), Sinv(kf%m, kf%m)) allocate(K(kf%n, kf%m), z_pred(kf%m), y(kf%m), IKH(kf%n, kf%n)) ! 计算增益 K = P·H^T·(H·P·H^T + R)^(-1) PHt = matmul(kf%P, transpose(kf%H)) S = matmul(kf%H, PHt) + kf%R call mat_inv(S, Sinv, kf%m) K = matmul(PHt, Sinv) ! 新息与实际状态修正 z_pred = matmul(kf%H, kf%x) y = z - z_pred kf%x = kf%x + matmul(K, y) ! 协方差更新 P = (I - K·H)·P IKH = -matmul(K, kf%H) do i = 1, kf%n IKH(i,i) = IKH(i,i) + 1.0_dp end do kf%P = matmul(IKH, kf%P) deallocate(PHt, S, Sinv, K, z_pred, y, IKH) end subroutine kf_update

这段代码里最值得留意的是协方差更新那一步。教科书上写 P = (I - K·H)·P，代码里我先算出 IKH = -K·H，再把对角线加 1，等价于得到 I - K·H。这样做的原因是 Fortran 的数组整体运算不容易直接表达“单位阵减一个矩阵”，拆开写反而清晰。新息 y 的顺序是 z 减去预测观测，方向一定不能反，反了估计值会往错误方向修正。

配套的求逆子例程用高斯-约当消元加部分主元选择：

subroutine mat_inv(A, Ainv, n) integer, intent(in) :: n real(dp), intent(in) :: A(n,n) real(dp), intent(out) :: Ainv(n,n) real(dp) :: aug(n, 2*n) integer :: i, j, k real(dp) :: pivot aug(:, 1:n) = A aug(:, n+1:2*n) = 0.0_dp do i = 1, n aug(i, n+i) = 1.0_dp end do do k = 1, n pivot = aug(k,k) if (abs(pivot) < 1.0e-12_dp) then do i = k+1, n if (abs(aug(i,k)) > abs(pivot)) then call swap_row(aug, k, i, n) pivot = aug(k,k) end if end do end if aug(k,:) = aug(k,:) / pivot do i = 1, n if (i /= k) then aug(i,:) = aug(i,:) - aug(i,k) * aug(k,:) end if end do end do Ainv = aug(:, n+1:2*n) end subroutine mat_inv subroutine swap_row(a, i, j, n) integer, intent(in) :: i, j, n real(dp), intent(inout) :: a(n, 2*n) real(dp) :: tmp(2*n) tmp = a(i,:) a(i,:) = a(j,:) a(j,:) = tmp end subroutine swap_row

求逆里主元选择这一步不是可有可无的。如果 S 矩阵接近奇异，对角线元素接近零，直接除法会得到天文数字，后面协方差矩阵直接发散。部分主元能保证每一列都选绝对值最大的元素当除数，能扛住绝大多数实际场景。这里的阈值 1.0e-12_dp 是经验值，如果你的数据量级在千级或万级，先看 S 的对角线元素大小再调整这个阈值。

3.3 一维位置跟踪主程序：用模拟数据验证整个流程

主程序做的事情是：生成一条带噪声的位置观测序列，用卡尔曼滤波估计位置和速度，最后计算位置估计的 RMSE。真实轨迹用匀加速模型生成，这样既考验 F 矩阵的建模能力，也让 Q 矩阵的设置有一个明确物理对照。

program kf_demo use kf_module, only: dp, kf_state, kf_predict, kf_update implicit none type(kf_state) :: kf real(dp), parameter :: dt = 0.1_dp real(dp), parameter :: acc_true = 0.1_dp ! 真实加速度 m/s^2 real(dp), parameter :: sigma_r = 1.0_dp ! 观测噪声标准差 m real(dp), parameter :: q_acc = 0.01_dp ! 过程噪声强度 integer, parameter :: nstep = 200 real(dp) :: x_true, v_true, z, gauss_rng real(dp) :: rmse, sum_sq = 0.0_dp integer :: i call random_seed() kf%n = 2 kf%m = 1 allocate(kf%F(2,2), kf%H(1,2), kf%Q(2,2), kf%R(1,1), kf%P(2,2), kf%x(2)) kf%F = reshape([1.0_dp, 0.0_dp, dt, 1.0_dp], [2,2]) kf%H = reshape([1.0_dp, 0.0_dp], [1,2]) kf%R(1,1) = sigma_r * sigma_r kf%Q = reshape([q_acc*dt**3/3.0_dp, q_acc*dt**2/2.0_dp, & q_acc*dt**2/2.0_dp, q_acc*dt], [2,2]) kf%P = reshape([100.0_dp, 0.0_dp, 0.0_dp, 100.0_dp], [2,2]) kf%x = [0.0_dp, 1.0_dp] x_true = 0.0_dp v_true = 1.0_dp do i = 1, nstep x_true = x_true + v_true * dt v_true = v_true + acc_true * dt z = x_true + gauss_rng() * sigma_r call kf_predict(kf) call kf_update(kf, [z]) sum_sq = sum_sq + (kf%x(1) - x_true)**2 end do rmse = sqrt(sum_sq / dble(nstep)) write(*, '(A,F8.3)') 'position RMSE = ', rmse contains function gauss_rng() result(g) real(dp) :: g, u1, u2, pi call random_number(u1) call random_number(u2) pi = 4.0_dp * atan(1.0_dp) g = sqrt(-2.0_dp * log(u1)) * cos(2.0_dp * pi * u2) end function gauss_rng end program kf_demo

主程序里有几个参数值得关注。q_acc 是过程噪声强度，它对滤波器的跟踪能力影响最大：q_acc 太小，滤波器会过于相信预测，响应迟钝；q_acc 太大，估计值会跟着观测噪声大幅抖动。sigma_r 是观测噪声标准差，你可以理解为传感器的测量精度，比如 GPS 位置测量精度在 1 米左右。初始 P 取 100，表示对初始位置和速度都不太确定，让滤波器在启动阶段更愿意听观测的。这些参数对真实项目来说都需要重新标定，但不能不设初值就空跑。

3.4 编译与结果判读：第一个能跑的卡尔曼滤波程序

编译命令很简单，用 gfortran 就可：

gfortran -O2 -Wall -o kf_demo kf_demo.f90 && ./kf_demo

如果一切正常，输出类似 position RMSE = 0.87 m。怎么判读这个数？观测噪声标准差是 1 米，滤波后的 RMSE 应该明显小于 1 米，在 0.6 到 0.9 之间都算正常。如果 RMSE 大于 1 米，说明滤波器没能有效利用运动模型；如果 RMSE 接近 0，反而要警惕过拟合，因为真实数据里不可能这么干净。

-Wall 选项建议一直开着。Fortran 的隐式类型声明是经典坑，变量名没声明就会被当成整数，-Wall 会把这些警告打印出来。第一次跑通后，可以试着把 q_acc 改成 0.0 再看 RMSE，你会发现估计值明显变迟钝，这就是 Q 矩阵作用的直观体现。

4. Q、R矩阵怎么调才不翻车：噪声建模与数值稳定性

上一章的代码能跑通，但离“能用于真实项目”还有距离。卡尔曼滤波在真实系统里翻车，绝大多数不是因为算法公式背错了，而是 Q 和 R 设得不符合物理实际，或者协方差矩阵在长时迭代中失去数值稳定性。

4.1 Q和R的物理意义与初值估算：从传感器数据反推

Q 描述的是模型误差，也就是你建立的 F 矩阵没捕捉到的真实扰动。比如匀加速模型里，真实目标可能忽然加减速，这个“忽然”就是过程噪声。R 描述的是传感器测量噪声，它只跟传感器本身有关，跟你建不建模没关系。

初值估算有个可靠套路：让传感器静止采集一组数据，算出标准差，平方后就是 R。这是最不玄学的来源，几乎所有惯性传感器、测距传感器都能这样标定。Q 的估算则相对麻烦，常见做法是先设一个偏大的值让滤波器“跟得上”，再逐步减小到响应和噪声都能接受的范围。

注意：Q 和 R 的量纲必须一致。位置观测对应的 R 是米平方，那么 Q 的位置分量也应该是米平方，不能拿厘米标定的 R 和米建模的 Q 直接放一起算，否则增益计算会失真。

4.2 协方差对称性与发散检测：给矩阵上两道保险

卡尔曼滤波理论要求 P 始终是对称正定矩阵。但在双精度浮点运算下，每一次矩阵乘法和求逆都会引入微小的舍入误差，几十步迭代后 P 可能不对称，甚至对角线出现负值。负的对角线意味着方差为负，这在物理上荒谬，数值上则会在下一步预测时让 matmul 结果走向 NaN。

我习惯在每个更新步结束后加两个保险，代码片段如下：

! 保险一：强制对称化，消除浮点舍入造成的不对称 kf%P = 0.5_dp * (kf%P + transpose(kf%P)) ! 保险二：NaN检测，P 中任一元素为 NaN 立即重置 if (any(kf%P /= kf%P)) then write(*,*) 'Warning: P contains NaN, resetting P' kf%P = 0.0_dp do i = 1, kf%n kf%P(i,i) = 100.0_dp end do end if

这段代码里kf%P /= kf%P的写法利用了 NaN 不等于任何数包括自身的特性，在 Fortran 里也适用。重置的 100.0 不是通用值，它应该等于你对状态初值不确定度的估计。这里要说明白：重置 P 是救命手段不是治疗手段，它只能让你从崩溃中点重启，真正的原因还要靠下面几章排查。

对称化这个操作，理论上是把一个非对称矩阵投影到对称空间，代价是轻微的统计信息损失。但相对于数值崩坏导致的后果，这个代价完全可以接受。我见过一个跑了两小时才发散的案例，加了对称化之后连续跑一个礼拜都没出问题。

4.3 数值稳定性的三个升级方向：Joseph形式、平方根滤波、双精度

如果你的滤波器是长时间连续运行的，比如导航系统一天跑下来几百万步，标准更新公式 P = (I - K·H)·P 的舍入误差累积会越来越明显。这时候有三个升级方向。

第一个方向是改用 Joseph 形式：

P_est = (I - K·H)·P_pred·(I - K·H)^T + K·R·K^T

这个形式能保证 P_est 在理论上严格对称正定，代价是多算两次矩阵乘法和一次转置。实现代码为：

! Joseph 形式更新协方差，代价略高但数值稳定性更好 P_new = matmul(IKH, matmul(kf%P, transpose(IKH))) & + matmul(K, matmul(kf%R, transpose(K))) kf%P = P_new

第二个方向是平方根滤波，也就是把 P 分解成 S·S^T，迭代过程中只更新 S，避免直接对 P 做减法运算。这个方向实现复杂度高，适合对数值稳定性有极致要求的场景，普通项目不需要。第三个方向最简单，就是全程序坚持用双精度，也就是我们已经在用的 dp = selected_real_kind(15, 307)。很多翻车案例其实是单双精度混用导致的，比如某个中间变量被声明成默认 real，与 dp 运算时被静默转换成单精度，精度直接掉到 7 位有效数字。

5. Fortran卡尔曼滤波常见问题排查：五个必踩的坑和解决办法

这一章直接按踩坑记录写，每条都是“现象 → 原因 → 解决”的结构。这些坑我在给不同项目做代码审查时反复见过，自己也亲手踩过几个。

5.1 现象：协方差矩阵发散成NaN或天文数字

跑了一段时间后，P 矩阵的元素变成 NaN，或者exp(几十) 这样的天文数字，滤波结果直接不可用。

原因基本有三个：一是 S = H·P·H^T + R 接近奇异，求逆时出现除零；二是初始 P 设置不合理，导致增益本来就过大；三是 Q 矩阵被设成零，预测步让协方差持续收缩到失去修正能力。

解决步骤是：先加 4.2 节的 NaN 检测，确认发散发生的时刻，通常能对应到某个观测突变的瞬间。然后打印 S 矩阵的对角线元素，如果某个值小于 1e-10，考虑加一个很小的对角扰动，比如 S = S + 1e-8·I。最后检查 Q 矩阵，确保它是对称正定的，且对角线元素不是零。

5.2 现象：增益震荡、估计值抖动

增益 K 在某个阈值附近来回跳，估计值也跟着抖，曲线像锯齿。

最常见原因是 R 设得太小。R 小意味着你认为传感器很准，增益会接近 1，观测噪声几乎全部进入估计结果。另一个原因是观测数据本身存在粗差，也就是野值，卡尔曼滤波对野值没有天然抵抗力。

解决方法是先用 4.1 节的静态数据法重新标定 R，不要凭感觉填一个很小值。如果野值频发，可以增加一个新息的判断：当 |z - H·x_pred| 超过 3 倍新息标准差时，跳过这次更新或者加大 R。这个技巧不复杂，但对工程系统的可靠性提升非常明显。

5.3 现象：估计滞后于真实轨迹

目标明明在匀速运动，滤波结果却始终慢半拍，而且滞后量随着速度增大而增大。

原因基本锁定在状态转移矩阵 F 上。匀加速模型里 F 的右上角元素应该是 dt，如果写代码时把 dt 和 1 的位置搞反，或者漏掉了速度到位置的耦合项，预测步根本推不动位置。另一个原因是 Q 设得太小，滤波器宁愿相信滞后的预测，也不愿意被观测拉动。

解决方法是把 F 矩阵打印出来逐项核对。二维匀速运动模型的 F 应该是 [1, dt; 0, 1]，含义是当前时刻的位置等于上一时刻位置加上速度乘时间。如果确认 F 没问题，再检查控制输入项 B·u 是否被遗漏，比如目标本身有恒定加速度，但你只在观测里看到了结果而模型里没有对应的输入项。

5.4 现象：数组越界和segmentation fault

程序一跑起来就崩，或者在某一步突然段错误。这个现象在 Fortran 里特别容易迷惑人，因为报错行往往不是真正出错的行。

原因大多数是维度不匹配。Fortran 的数组索引从 1 开始，reshape 按列主序填充，很多从教材公式转写代码的人习惯按行思考，导致矩阵实际形状和预期不一致。比如 reshap([1, 0, dt, 1], [2,2]) 得到的是列主序排列，如果忘记这一点，H 矩阵很可能被填成转置的形状，matmul 直接报维度错误。

解决方法是养成两个习惯：一是在每个子例程入口用 size 函数打印或断言数组维度；二是把矩阵初始化独立成一个子例程，所有 reshape 参数统一管理。我一般会写一个调试用的 dump_matrix 子例程，一旦崩了就把所有矩阵打印出来对照 paper 上的维度。

5.5 现象：与C、Python混编时结果不对

整个 Fortran 模块单独测试正常，但通过 C 或 Python 调用之后，结果全乱。

原因是多方面的，最典型的是数据布局差异。Fortran 矩阵是列主序存储，C 是行主序，直接当成同样内存布局传给 C 函数，所有元素位置错乱。第二个原因是 kind 精度不匹配，Fortran 的 dp 是 8 字节双精度，对应 C 的 double；如果你在 C 侧用了 float，数据被截断成单精度。第三个原因是函数名 mangling，Fortran 编译器通常会在子例程名后加下划线或改变大小写，C 函数声明也得对应修改。

解决方法是使用 ISO_C_BINDING 特性，在 Fortran 子例程里显式声明 bind(C)，并让所有传递的矩阵降维成一维数组再传址。示意的声明方式如下：

subroutine kf_update_c(kf_ptr, z, n_total) bind(C, name="kf_update_c") use iso_c_binding, only: c_ptr, c_double implicit none type(c_ptr), intent(in) :: kf_ptr real(c_double), intent(in) :: z(*) integer :: n_total ! 这里用 c_f_pointer 把 kf_ptr 转回 kf_state 类型指针 end subroutine

这个方向展开写又是一篇长文，这里只给出手法：混编代码的调试成本往往高于重写部分接口函数，如果你的调用链允许，尽量把 Fortran 模块包成独立可执行程序，用文件或内存映射做数据交换，能绕开大半混编问题。

6. 让卡尔曼滤波程序真正可用：接口设计、验证与一个实用技巧

6.1 把模块做成可复用库：初始化与多目标支持

最小实现跑通后，我做的最重要重构是增加一个 kf_init 子例程，把主程序里一堆 allocate 和 reshape 收口到一处。接口类似这样：

subroutine kf_init(kf, n, m, F, H, Q, R, x0, P0) type(kf_state), intent(inout) :: kf integer, intent(in) :: n, m real(dp), intent(in) :: F(n,n), H(m,n), Q(n,n), R(m,m) real(dp), intent(in) :: x0(n), P0(n,n) ! 内部统一 allocate 并赋值 end subroutine

这个修改让多目标跟踪变得很干净：每个目标一个 kf_state 实例，各自调用 kf_init，互不干扰。我参与过的一个模拟项目需要在同一进程里跟踪多个目标，最初在循环里反复手动 allocate 导致内存碎片，重构后每个目标实例化一次，稳定性和代码可读性都好很多。

6.2 验证滤波效果的三个指标：RMSE、新息白噪声、蒙特卡洛

程序写完了，怎么证明它对？三个指标依次检查。第一是 RMSE，模拟场景已知真值，计算估计值与真值的均方根误差，这个我们已用。第二是新息白噪声，计算每步新息 y 除以理论标准差 sqrt(S)，得到归一化新息序列，它的自相关系数应该在零附近波动。如果自相关明显不为零，说明模型 F 或者 Q 还有没捕捉到的动态。第三是蒙特卡洛，同一场景跑 20 次不同随机种子，统计估计误差的均值和方差，均值应接近零，说明滤波器无偏；方差应与 P 矩阵对角线吻合，说明协方差的估计本身是可信的。

提示：新息白噪声检查是最容易被忽略的验证手段。它不需要真值，只要观测序列就能做，非常适合用真实数据验证滤波器是否还有未建模动态。

我现在的习惯是，任何卡尔曼滤波程序写完后先跑一遍模拟数据，再把新息序列的均值打印出来看一眼。如果均值偏离零超过一个标准差，我会先怀疑状态转移矩阵，再怀疑 Q，最后才怀疑观测数据本身。这套排查顺序帮我在几次项目里省掉了大量瞎调参的时间。卡尔曼滤波调参确实有一半玄学成分，但矩阵维度、量纲一致性和新息统计是三个稳定的抓手，把这三点守住了，另一半玄学也只是搜索空间里的小扰动而已。希望帮到你。

本文还有配套的精品资源，点击获取