【自适应滤波】基于新息协方差匹配的自适应CKF目标跟踪 MATLAB 实战——在目标跟踪、雷达定位、组合导航和传感器融合等问题

如有代码定制、讲解的需求，可联系我↑

在目标跟踪、雷达定位、组合导航和传感器融合等问题中，卡尔曼滤波是一类非常常用的状态估计方法。对于线性系统，经典 Kalman Filter 已经可以取得不错的效果；但在很多实际场景中，系统往往并不是线性的，比如雷达测距测角、无人机定位、车辆运动跟踪等，这时候就需要使用 EKF、UKF、CKF 等非线性滤波方法。

这篇文章分享的是一个基于 MATLAB 的自适应容积卡尔曼滤波例程，主要实现二维雷达目标跟踪任务。代码中同时给出了经典 CKF 和基于新息协方差匹配的自适应 CKF，也就是 CM-ACKF，并通过仿真结果对比两种方法在目标轨迹估计、位置误差、速度误差和噪声参数调整方面的表现。

本例程的一个重点是：即使初始过程噪声协方差 Q 和观测噪声协方差 R 设置得不够准确，CM-ACKF 也可以利用新息信息进行在线调整，从而让滤波器逐渐适应真实噪声环境。

程序实现的主要功能

本文代码主要完成以下内容：

构建二维平面目标运动模型；
构建雷达测距、测角非线性观测模型；
实现经典 CKF 容积卡尔曼滤波；
实现基于新息协方差匹配的自适应 CKF；
对过程噪声 Q 和观测噪声 R 进行在线调节；
对比真实轨迹、雷达观测、CKF 估计和 CM-ACKF 估计；
输出位置误差、速度误差、RMSE 和性能提升统计结果；
绘制 Q、R 自适应变化曲线，观察滤波器的自我修正过程。

运行代码后，可以直观看到 CM-ACKF 相比经典 CKF 在噪声参数不准确时的优势。经典 CKF 使用固定 Q 和 R，一旦噪声设置偏差较大，滤波精度就会受到影响；而 CM-ACKF 可以根据新息统计结果逐步修正 Q 和 R，使滤波器更加贴合真实系统。

状态模型设计

仿真对象是二维平面内运动的目标，状态向量设置为：

x ∗ k = $x k v * x , k y k v y , k$ \mathbf{x}*k = \begin{bmatrix} x_k \ v*{x,k} \ y_k \ v_{y,k} \end{bmatrix} x∗k= $xk v*x,k yk vy,k$

其中：

x k x_k xk 表示目标在 X 方向的位置；
v x , k v_{x,k} vx,k 表示目标在 X 方向的速度；
y k y_k yk 表示目标在 Y 方向的位置；
v y , k v_{y,k} vy,k 表示目标在 Y 方向的速度。

本文采用匀速运动模型，状态转移方程为：

x ∗ k = F x ∗ k − 1 + G w k − 1 \mathbf{x}*k = \mathbf{F}\mathbf{x}*{k-1} + \mathbf{G}\mathbf{w}_{k-1} x∗k=Fx∗k−1+Gwk−1

其中，状态转移矩阵为：

F = $1 Δ t 0 0 0 1 0 0 0 0 1 Δ t 0 0 0 1$ \mathbf{F} = \begin{bmatrix} 1 & \Delta t & 0 & 0 \ 0 & 1 & 0 & 0 \ 0 & 0 & 1 & \Delta t \ 0 & 0 & 0 & 1 \end{bmatrix} F= $1Δt00 0100 001Δt 0001$

过程噪声输入矩阵为：

G = $Δ t 2 2 0 Δ t 0 0 Δ t 2 2 0 Δ t$ \mathbf{G} = \begin{bmatrix} \frac{\Delta t^2}{2} & 0 \ \Delta t & 0 \ 0 & \frac{\Delta t^2}{2} \ 0 & \Delta t \end{bmatrix} G= $2Δt20 Δt0 02Δt2 0Δt$

过程噪声满足：

w k − 1 ∼ N ( 0 , q 2 I ) \mathbf{w}_{k-1} \sim \mathcal{N}(0, q^2\mathbf{I}) wk−1∼N(0,q2I)

因此，真实过程噪声协方差可以写成：

Q t r u e = q 2 G G T \mathbf{Q}_{true} = q^2 \mathbf{G}\mathbf{G}^T Qtrue=q2GGT

这个模型的物理意义比较直观：目标在 X 和 Y 两个方向上做近似匀速运动，但受到随机加速度噪声影响，因此实际轨迹并不是完全理想的直线。

雷达非线性观测模型

本例程中，雷达可以测得目标相对于雷达位置的距离和方位角。假设雷达位置为：

p r = $x r y r$ \mathbf{p}_r = \begin{bmatrix} x_r \ y_r \end{bmatrix} pr= $xr yr$

目标位置为：

p k = $x k y k$ \mathbf{p}_k = \begin{bmatrix} x_k \ y_k \end{bmatrix} pk= $xk yk$

则雷达距离观测为：

r k = ( x k − x r ) 2 + ( y k − y r ) 2 r_k = \sqrt{(x_k - x_r)^2 + (y_k - y_r)^2} rk=(xk−xr)2+(yk−yr)2

雷达方位角观测为：

θ k = arctan ⁡ 2 ( y k − y r , x k − x r ) \theta_k = \arctan2(y_k - y_r, x_k - x_r) θk=arctan2(yk−yr,xk−xr)

因此，系统观测方程可以写成：

z k = h ( x k ) + v k \mathbf{z}_k = h(\mathbf{x}_k) + \mathbf{v}_k zk=h(xk)+vk

其中：

h ( x k ) = $( x k - x r ) 2 + ( y k - y r ) 2 arctan 2 ( y k - y r , x k - x r )$ h(\mathbf{x}_k) = \begin{bmatrix} \sqrt{(x_k - x_r)^2 + (y_k - y_r)^2} \ \arctan2(y_k - y_r, x_k - x_r) \end{bmatrix} h(xk)= $(xk-xr)2+(yk-yr)2 arctan2(yk-yr,xk-xr)$

观测噪声满足：

v k ∼ N ( 0 , R ) \mathbf{v}_k \sim \mathcal{N}(0,\mathbf{R}) vk∼N(0,R)

观测噪声协方差矩阵为：

R = $σ r 2 0 0 σ θ 2$ \mathbf{R} = \begin{bmatrix} \sigma_r^2 & 0 \ 0 & \sigma_{\theta}^2 \end{bmatrix} R= $σr20 0σθ2$

这里的观测模型是非线性的，因为它包含平方根函数和反正切函数。如果直接使用普通 Kalman Filter，就无法处理这种非线性观测关系。因此，本文采用 CKF 容积卡尔曼滤波进行状态估计。

CKF 容积卡尔曼滤波基本思想

CKF 的核心思想是：不用直接对非线性函数做线性化，而是在状态空间中选取一组容积点，将这些点通过非线性函数传播，然后利用传播后的点来近似计算均值和协方差。

对于 n 维状态变量，CKF 会选取 2 n 2n 2n 个容积点。本文状态维度为：

n = 4 n = 4 n=4

所以容积点个数为：

m = 2 n = 8 m = 2n = 8 m=2n=8

单位容积点可以表示为：

ξ i = { n e ∗ i , i = 1 , 2 , ⋯ , n − n e ∗ i − n , i = n + 1 , n + 2 , ⋯ , 2 n \boldsymbol{\xi}_i = \begin{cases} \sqrt{n}\mathbf{e}*i, & i=1,2,\cdots,n \ -\sqrt{n}\mathbf{e}*{i-n}, & i=n+1,n+2,\cdots,2n \end{cases} ξi={n e∗i,i=1,2,⋯,n −n e∗i−n,i=n+1,n+2,⋯,2n

其中， e i \mathbf{e}_i ei 表示单位向量。

实际容积点由状态均值和协方差生成：

X i , k = x ^ k + S k ξ i \mathbf{X}_{i,k} = \hat{\mathbf{x}}_k + \mathbf{S}_k\boldsymbol{\xi}_i Xi,k=x^k+Skξi

其中 S k \mathbf{S}_k Sk 是协方差矩阵 P k \mathbf{P}_k Pk 的 Cholesky 分解结果：

P k = S k S k T \mathbf{P}_k = \mathbf{S}_k\mathbf{S}_k^T Pk=SkSkT

这些容积点可以理解为围绕当前估计状态分布的一组采样点。协方差越大，容积点分布得越分散；协方差越小，容积点越集中。

CKF 预测过程

在预测阶段，先利用状态转移方程传播状态：

x ^ ∗ k ∣ k − 1 = F x ^ ∗ k − 1 ∣ k − 1 \hat{\mathbf{x}}*{k|k-1} = \mathbf{F}\hat{\mathbf{x}}*{k-1|k-1} x^∗k∣k−1=Fx^∗k−1∣k−1

协方差预测为：

P ∗ k ∣ k − 1 = F P ∗ k − 1 ∣ k − 1 F T + Q \mathbf{P}*{k|k-1} = \mathbf{F}\mathbf{P}*{k-1|k-1}\mathbf{F}^T + \mathbf{Q} P∗k∣k−1=FP∗k−1∣k−1FT+Q

然后基于预测状态和预测协方差生成容积点：

X ∗ i , k ∣ k − 1 = x ^ ∗ k ∣ k − 1 + S k ∣ k − 1 ξ i \mathbf{X}*{i,k|k-1} = \hat{\mathbf{x}}*{k|k-1} + \mathbf{S}_{k|k-1}\boldsymbol{\xi}_i X∗i,k∣k−1=x^∗k∣k−1+Sk∣k−1ξi

其中：

P ∗ k ∣ k − 1 = S ∗ k ∣ k − 1 S k ∣ k − 1 T \mathbf{P}*{k|k-1} = \mathbf{S}*{k|k-1}\mathbf{S}_{k|k-1}^T P∗k∣k−1=S∗k∣k−1Sk∣k−1T

在这个过程中，Q 的大小会直接影响预测协方差。如果 Q 设置太小，滤波器会过度相信运动模型；如果 Q 设置太大，滤波器又会认为目标运动非常不稳定，导致估计结果抖动。因此，Q 的合理设置对滤波效果非常关键。

CKF 更新过程

在更新阶段，需要将预测容积点代入雷达观测方程：

Z ∗ i , k ∣ k − 1 = h ( X ∗ i , k ∣ k − 1 ) \mathbf{Z}*{i,k|k-1} = h(\mathbf{X}*{i,k|k-1}) Z∗i,k∣k−1=h(X∗i,k∣k−1)

预测观测均值为：

z ^ ∗ k ∣ k − 1 = 1 m ∑ ∗ i = 1 m Z i , k ∣ k − 1 \hat{\mathbf{z}}*{k|k-1} = \frac{1}{m}\sum*{i=1}^{m}\mathbf{Z}_{i,k|k-1} z^∗k∣k−1=m1∑∗i=1mZi,k∣k−1

预测观测协方差为：

P ∗ z z , k = 1 m ∑ ∗ i = 1 m ( Z ∗ i , k ∣ k − 1 − z ^ ∗ k ∣ k − 1 ) ( Z ∗ i , k ∣ k − 1 − z ^ ∗ k ∣ k − 1 ) T ∗ R \mathbf{P}*{zz,k} = \frac{1}{m}\sum*{i=1}^{m} (\mathbf{Z}*{i,k|k-1}-\hat{\mathbf{z}}*{k|k-1}) (\mathbf{Z}*{i,k|k-1}-\hat{\mathbf{z}}*{k|k-1})^T* \mathbf{R} P∗zz,k=m1∑∗i=1m(Z∗i,k∣k−1−z^∗k∣k−1)(Z∗i,k∣k−1−z^∗k∣k−1)T∗R

状态与观测之间的互协方差为：

P ∗ x z , k = 1 m ∑ ∗ i = 1 m ( X ∗ i , k ∣ k − 1 − x ^ ∗ k ∣ k − 1 ) ( Z ∗ i , k ∣ k − 1 − z ^ ∗ k ∣ k − 1 ) T \mathbf{P}*{xz,k} = \frac{1}{m}\sum*{i=1}^{m} (\mathbf{X}*{i,k|k-1}-\hat{\mathbf{x}}*{k|k-1}) (\mathbf{Z}*{i,k|k-1}-\hat{\mathbf{z}}*{k|k-1})^T P∗xz,k=m1∑∗i=1m(X∗i,k∣k−1−x^∗k∣k−1)(Z∗i,k∣k−1−z^∗k∣k−1)T

然后计算卡尔曼增益：

K ∗ k = P ∗ x z , k P z z , k − 1 \mathbf{K}*k = \mathbf{P}*{xz,k}\mathbf{P}_{zz,k}^{-1} K∗k=P∗xz,kPzz,k−1

新息为：

ν k = z ∗ k − z ^ ∗ k ∣ k − 1 \boldsymbol{\nu}_k = \mathbf{z}*k - \hat{\mathbf{z}}*{k|k-1} νk=z∗k−z^∗k∣k−1

状态更新为：

x ^ ∗ k ∣ k = x ^ ∗ k ∣ k − 1 + K k ν k \hat{\mathbf{x}}*{k|k} = \hat{\mathbf{x}}*{k|k-1} + \mathbf{K}_k\boldsymbol{\nu}_k x^∗k∣k=x^∗k∣k−1+Kkνk

协方差更新为：

P ∗ k ∣ k = P ∗ k ∣ k − 1 − K ∗ k P ∗ z z , k K k T \mathbf{P}*{k|k} = \mathbf{P}*{k|k-1} - \mathbf{K}*k\mathbf{P}*{zz,k}\mathbf{K}_k^T P∗k∣k=P∗k∣k−1−K∗kP∗zz,kKkT

这里需要注意，由于观测中包含角度 θ \theta θ，所以在计算角度残差时需要进行归一化处理：

Δ θ = atan2 ⁡ ( sin ⁡ ( Δ θ ) , cos ⁡ ( Δ θ ) ) \Delta \theta = \operatorname{atan2}(\sin(\Delta \theta), \cos(\Delta \theta)) Δθ=atan2(sin(Δθ),cos(Δθ))

这样可以把角度误差限制在 $- π , π$ $-\\pi,\\pi$ $-π,π$ 范围内，避免出现例如 179° 和 -179° 被误判为相差 358° 的问题。

7. 为什么需要自适应 Q 和 R？

经典 CKF 的问题在于：Q 和 R 一旦给定，在整个滤波过程中就保持不变。

但在实际应用中，Q 和 R 往往很难准确设置。

例如：

雷达测距误差可能会随着距离变化；
测角精度可能会受到目标方位、遮挡或噪声干扰影响；
目标运动状态可能不是严格匀速；
初始噪声参数可能只是经验值；
实际环境可能比仿真模型更加复杂。

如果 Q 设置不合理，滤波器对运动模型的信任程度就会出现偏差。如果 R 设置不合理，滤波器对观测数据的信任程度也会出现偏差。

一般来说：

当 R 设置过小，滤波器会过度相信观测，估计结果容易被测量噪声带偏；

当 R 设置过大，滤波器会过度相信预测，估计结果可能出现滞后；

当 Q 设置过小，滤波器认为目标运动很稳定，面对机动变化时响应较慢；

当 Q 设置过大，滤波器认为目标运动随机性很强，估计结果可能变得不够平滑。

因此，本文采用新息协方差匹配方法，让滤波器根据实际运行过程中的新息变化来自动修正 Q 和 R。

新息协方差匹配原理

新息定义为：

ν k = z ∗ k − z ^ ∗ k ∣ k − 1 \boldsymbol{\nu}_k = \mathbf{z}*k - \hat{\mathbf{z}}*{k|k-1} νk=z∗k−z^∗k∣k−1

它表示实际观测和预测观测之间的差值。理论上，新息协方差应该接近：

S ∗ k = P ∗ z z , k \mathbf{S}*k = \mathbf{P}*{zz,k} S∗k=P∗zz,k

也就是滤波器根据当前 Q、R 计算出的理论新息协方差。

但在实际运行中，可以用最近 M 个时刻的新息计算实际新息协方差：

C ∗ k = 1 M ∑ ∗ i = k − M + 1 k ν i ν i T \mathbf{C}*k = \frac{1}{M} \sum*{i=k-M+1}^{k} \boldsymbol{\nu}_i\boldsymbol{\nu}_i^T C∗k=M1∑∗i=k−M+1kνiνiT

其中 M 是滑动窗口长度。本文代码中使用滑动窗口统计最近一段时间的新息变化，而不是只看单个时刻的新息，这样可以减少偶然噪声对参数调整的影响。

自适应调节的基本判断逻辑是：

C k > S k \mathbf{C}_k > \mathbf{S}_k Ck>Sk

说明实际新息比理论预测更大，当前滤波器可能低估了噪声，需要适当增大 Q 或 R。

C k < S k \mathbf{C}_k < \mathbf{S}_k Ck<Sk

说明实际新息比理论预测更小，当前滤波器可能高估了噪声，需要适当减小 Q 或 R。

这就是新息协方差匹配的核心思想：让实际新息统计结果和理论新息协方差尽量匹配。

观测噪声 R 的自适应调整

本文代码中，R 的调整采用平滑更新方式。首先计算实际新息协方差与理论新息协方差之间的差值：

Δ R k = C k − S k \Delta \mathbf{R}_k = \mathbf{C}_k - \mathbf{S}_k ΔRk=Ck−Sk

然后对 R 进行更新：

R ∗ k = α R R ∗ k − 1 + ( 1 − α R ) ( R k − 1 + Δ R k ) \mathbf{R}*k = \alpha_R \mathbf{R}*{k-1} + (1-\alpha_R)(\mathbf{R}_{k-1}+\Delta \mathbf{R}_k) R∗k=αRR∗k−1+(1−αR)(Rk−1+ΔRk)

其中， α R \alpha_R αR 是平滑因子。

当 α R \alpha_R αR 较大时，R 调整较平稳，不容易因为单次异常观测发生剧烈变化；

当 α R \alpha_R αR 较小时，R 调整更快，但也更容易受到噪声扰动。

本文代码中使用：

α R = 0.95 \alpha_R = 0.95 αR=0.95

这表示每次更新时大部分保留原来的 R，只引入少量新息统计修正量。这种方式比较稳健，适合演示自适应滤波的收敛过程。

为了保证数值稳定性，代码中还会对 R 进行对称化处理：

R k = R k + R k T 2 \mathbf{R}_k = \frac{\mathbf{R}_k + \mathbf{R}_k^T}{2} Rk=2Rk+RkT

并加入很小的正则项，避免矩阵奇异：

R k = R k + ε I \mathbf{R}_k = \mathbf{R}_k + \varepsilon \mathbf{I} Rk=Rk+εI

过程噪声 Q 的自适应调整

相比 R，Q 的调整通常更加谨慎。因为 Q 描述的是系统运动模型的不确定性，如果调整过快，可能会导致滤波器状态预测部分不稳定。

本文代码使用迹比值来判断是否需要调整 Q：

ρ k = tr ⁡ ( C k ) tr ⁡ ( S k ) \rho_k = \frac{\operatorname{tr}(\mathbf{C}_k)} {\operatorname{tr}(\mathbf{S}_k)} ρk=tr(Sk)tr(Ck)

其中， tr ⁡ ( ⋅ ) \operatorname{tr}(\cdot) tr(⋅) 表示矩阵的迹，也就是矩阵对角线元素之和。

当：

ρ k > ρ u p p e r \rho_k > \rho_{upper} ρk>ρupper

说明实际新息整体偏大，需要增大 Q；

当：

ρ k < ρ l o w e r \rho_k < \rho_{lower} ρk<ρlower

说明实际新息整体偏小，需要减小 Q；

当：

ρ l o w e r ≤ ρ k ≤ ρ u p p e r \rho_{lower} \leq \rho_k \leq \rho_{upper} ρlower≤ρk≤ρupper

说明当前理论新息和实际新息差异不大，可以保持 Q 不变。

本文代码中设置：

ρ u p p e r = 1.2 \rho_{upper} = 1.2 ρupper=1.2

ρ l o w e r = 0.8 \rho_{lower} = 0.8 ρlower=0.8

也就是说，当实际新息协方差和理论新息协方差的差异在一定范围内时，不进行调整，避免 Q 来回波动。

Q 的平滑更新形式可以写成：

Q ∗ k = α Q Q ∗ k − 1 + ( 1 − α Q ) λ k Q k − 1 \mathbf{Q}*k = \alpha_Q \mathbf{Q}*{k-1} + (1-\alpha_Q)\lambda_k\mathbf{Q}_{k-1} Q∗k=αQQ∗k−1+(1−αQ)λkQk−1

其中， λ k \lambda_k λk 是根据 ρ k \rho_k ρk 得到的调整比例。代码中还对调整倍数进行了限制：

λ k ∈ $0.5 , 2$ \lambda_k \in $0.5,2$ λk∈ $0.5,2$

这样可以避免 Q 在某一时刻突然变得过大或过小。

本文代码中使用：

α Q = 0.98 \alpha_Q = 0.98 αQ=0.98

可以看到，Q 的调整比 R 更慢、更保守。这也符合实际滤波调参经验：观测噪声可以相对快一点修正，而过程噪声通常需要更平滑地变化。

CM-ACKF 的整体流程

整个 CM-ACKF 的运行流程可以概括为：

设置目标初始状态、雷达位置、采样时间和仿真步数；
构建匀速运动模型和雷达非线性观测模型；
初始化状态估计、协方差矩阵、Q 和 R；
生成真实目标轨迹和带噪声雷达观测；
使用 CKF 进行状态预测；
生成容积点并通过雷达观测方程传播；
计算预测观测、新息、协方差和卡尔曼增益；
更新目标状态和协方差；
保存最近 M 步新息，计算实际新息协方差；
根据新息协方差匹配结果自适应调整 R；
根据迹比值自适应调整 Q；
输出估计轨迹、误差曲线和性能指标。

用更简洁的形式表示，就是：

预测 → 容积点传播 → 观测更新 → 新息统计 → 自适应调整Q/R \text{预测} \rightarrow \text{容积点传播} \rightarrow \text{观测更新} \rightarrow \text{新息统计} \rightarrow \text{自适应调整Q/R} 预测→容积点传播→观测更新→新息统计→自适应调整Q/R

相比经典 CKF，CM-ACKF 多出来的关键步骤就是最后的"新息统计"和"Q/R 自适应调整"。

12. 仿真结果展示

代码运行后，会依次生成多组图像结果。

第一类结果是轨迹对比图，包括真实轨迹、纯雷达观测点、经典 CKF 估计轨迹和 CM-ACKF 估计轨迹。通过这张图可以直观看到滤波器对目标运动轨迹的跟踪能力。

第二类结果是 X 轴和 Y 轴位置误差曲线。它可以反映不同方法在各个方向上的估计偏差。

第三类结果是目标整体位置误差曲线，计算方式为：

e p o s , k = ( x k − x ^ k ) 2 + ( y k − y ^ k ) 2 e_{pos,k} = \sqrt{ (x_k-\hat{x}_k)^2 + (y_k-\hat{y}_k)^2 } epos,k=(xk−x^k)2+(yk−y^k)2

第四类结果是速度误差曲线，计算方式为：

e v e l , k = ( v x , k − v ^ ∗ x , k ) 2 + ( v ∗ y , k − v ^ y , k ) 2 e_{vel,k} = \sqrt{ (v_{x,k}-\hat{v}*{x,k})^2 + (v*{y,k}-\hat{v}_{y,k})^2 } evel,k=(vx,k−v^∗x,k)2+(v∗y,k−v^y,k)2

RMSE 评价指标

为了评价滤波性能，代码中使用了 RMSE 指标。位置 RMSE 可以写成：

R M S E p o s = 1 N ∑ k = 1 N $( x k − x \^ k ) 2 + ( y k − y \^ k ) 2$ RMSE_{pos} = \sqrt{ \frac{1}{N} \sum_{k=1}^{N} \left $(x_k-\\hat{x}_k)\^2 + (y_k-\\hat{y}_k)\^2 \\right$ } RMSEpos=N1k=1∑N $(xk−x\^k)2+(yk−y\^k)2$

速度 RMSE 可以写成：

R M S E v e l = 1 N ∑ k = 1 N $( v x , k − v \^ ∗ x , k ) 2 + ( v ∗ y , k − v \^ y , k ) 2$ RMSE_{vel} = \sqrt{ \frac{1}{N} \sum_{k=1}^{N} \left $(v_{x,k}-\\hat{v}\*{x,k})\^2 + (v\*{y,k}-\\hat{v}_{y,k})\^2 \\right$ } RMSEvel=N1k=1∑N $(vx,k−v\^∗x,k)2+(v∗y,k−v\^y,k)2$

如果 CM-ACKF 的 RMSE 小于经典 CKF，就说明自适应调节确实改善了滤波效果。

程序中还计算了误差降低比例：

η = R M S E C K F − R M S E C M − A C K F R M S E C K F × 100 \eta = \frac{RMSE_{CKF} - RMSE_{CM-ACKF}} {RMSE_{CKF}} \times 100% η=RMSECKFRMSECKF−RMSECM−ACKF×100

这个指标可以更加直观地说明自适应方法相比固定参数 CKF 的提升幅度。

代码参数说明

本文代码中有几个比较重要的参数：

参数	含义	作用
`radar_pos`	雷达位置	决定测距和测角参考点
`dt`	采样时间	决定状态更新间隔
`N`	仿真步数	决定轨迹长度
`M`	滑动窗口长度	决定新息统计范围
`q_true`	真实过程噪声标准差	模拟目标运动随机性
`sigma_r`	距离测量噪声标准差	模拟雷达测距误差
`sigma_theta`	角度测量噪声标准差	模拟雷达测角误差
`Q_init_factor`	Q 初始估计倍数	测试 Q 设置不准时的自适应能力
`R_init_factor`	R 初始估计倍数	测试 R 设置不准时的自适应能力
`alpha_R`	R 平滑因子	控制 R 的调整速度
`alpha_Q`	Q 平滑因子	控制 Q 的调整速度
`trace_ratio_upper`	Q 调整上阈值	判断是否增大 Q
`trace_ratio_lower`	Q 调整下阈值	判断是否减小 Q

其中，M、alpha_R 和 alpha_Q 对自适应效果影响比较明显。

一般来说：

M 太小，新息统计不稳定，参数容易抖动；
M 太大，自适应响应变慢；
alpha 太小，调整速度快但容易受异常值影响；
alpha 太大，调整平稳但收敛较慢。

在实际使用中，可以根据目标运动强度和观测噪声变化情况进行微调。