【卡尔曼滤波】第2章非线性扩展与高级变体

张

张建站

2026/6/28 13:54:00

10分钟阅读

第2章非线性扩展与高级变体实际工程系统普遍呈现非线性动态特性标准卡尔曼滤波的线性假设面临根本性挑战。针对非线性状态估计问题扩展卡尔曼滤波器通过局部线性化逼近系统动态无迹卡尔曼滤波器则采用确定性采样策略捕获非线性变换的统计特性。信息滤波框架重构估计问题的对偶表示多模型方法处理系统模式的不确定性。本章系统阐述非线性估计理论的核心方法及其数值实现。sandbox:///mnt/kimi/output/chapter2_figures.png2.1 扩展卡尔曼滤波EKF2.1.1 非线性系统线性化考虑一般非线性离散时间系统其状态演化由非线性函数 f:Rn×Rp→Rn 描述xkf(xk−1,uk)wk−1观测方程通过非线性映射 h:Rn→Rm 建立zkh(xk)vk过程噪声与观测噪声保持高斯假设统计特性与线性系统一致。扩展卡尔曼滤波的核心在于围绕当前估计值进行一阶泰勒展开构造局部线性近似。状态转移函数的雅可比矩阵 Fk∈Rn×n 定义为Fk∂x∂fx^k−1∣k−1,uk其元素 [Fk]ij∂fi/∂xj 量化状态分量间的局部耦合强度。观测函数的雅可比矩阵 Hk∈Rm×n 类似定义Hk∂x∂hx^k∣k−1泰勒展开的高阶截断引入线性化误差。二阶余项涉及Hessian矩阵 ∇2fi 其范数 ∥R2∥≤21supξ∥∇2f(ξ)∥⋅∥x−x^∥2 表征近似精度。当非线性强度较高或估计误差较大时线性化误差主导滤波性能导致估计偏差甚至发散。2.1.2 EKF算法实现解析法实现要求推导雅可比矩阵的闭式表达式。对于刚体运动学 f(x)[pΔtv,vΔta]⊤ 雅可比矩阵呈现稀疏分块结构。符号计算工具如SymPy可自动生成导数代码避免手工求导错误但可能无法优化计算图效率。数值差分方法提供通用替代方案。前向差分近似[Fk]:,j≈δf(x^k−1∣k−1δej,uk)−f(x^k−1∣k−1,uk)截断误差 O(δ) 要求极小扰动 δ 但 machine epsilon 限制下舍入误差 O(ϵ/δ) 随 δ 减小而增大。中心差分[Fk]:,j≈2δf(x^δej)−f(x^−δej)以双倍计算代价换取 O(δ2) 精度适用于梯度计算敏感的离线应用。二阶扩展卡尔曼滤波引入Hessian矩阵修正均值传播x^k∣k−1f(x^k−1∣k−1)21∑i1ntr(∇2fiPk−1∣k−1)ei协方差预测相应包含二阶矩修正项。计算代价从 O(n3) 增至 O(n4) 限制其在实时系统中的应用。自适应EKF动态调整噪声协方差。基于新息序列的协方差匹配R^k(1−dk)R^k−1dk(y~ky~k⊤−HkPk∣k−1Hk⊤)遗忘因子 dk(1−b)/(1−bk1) 赋予近期观测更大权重b∈(0.9,0.999) 控制自适应速率。Sage-Husa估计器提供 Q 矩阵的在线辨识但可能破坏协方差正定性需投影修正。2.1.3 EKF局限性分析非线性强度可通过局部曲率张量评估。状态空间某点 x 处的可观测性矩阵O(x)∇h(x)∇h(x)∇f(x)⋮条件数 κ(O) 剧增指示线性化导致的可观测性退化。强非线性区域如坐标奇异点、饱和非线性中线性化近似失效估计误差协方差失去可靠性。初始估计敏感性表现为EKF对先验估计 x^0∣0 的强依赖。线性化点偏离真实状态将泰勒展开置于高阶项主导区域引发估计轨迹偏离。迭代EKF在观测更新中应用Gauss-Newton优化x^k∣k(i1)x^k∣k−1Kk(i)(zk−h(x^k∣k(i)))−Kk(i)Hk(i)(x^k∣k(i)−x^k∣k−1)迭代重线性化改善收敛性但增加计算负担。高斯假设在非线性变换后失效。即使输入为高斯分布输出呈现非对称、多峰特性。线性化传播仅匹配均值与协方差忽略高阶矩信息。新息序列的白噪声检验提供失效检测机制ϵky~k⊤Sk−1y~k∼χ2(m)累计 ∑ik−N1kϵi 超出 χ2 分布置信区间触发滤波发散警告启动重初始化或切换至鲁棒滤波模式。2.2 无迹卡尔曼滤波UKF2.2.1 Unscented TransformUT变换无迹变换通过确定性采样捕获概率分布经非线性映射后的统计特性。对于 n 维随机向量 x∼N(x^,P) 构造 2n1 个Sigma点χ0x^,W0(m)nλλ,W0(c)nλλ(1−α2β)χix^((nλ)P)i,Wi(m)Wi(c)2(nλ)1,i1,…,nχinx^−((nλ)P)i,Win(m)Win(c)2(nλ)1其中 λα2(nκ)−n 为尺度参数(P)i 表示矩阵平方根的第 i 列通常通过Cholesky分解获得。比例修正参数 α 控制Sigma点分布半径通常 10−4≤α≤1 减小 α 降低高阶项影响但可能引发数值问题β 引入先验分布峰度信息高斯分布取 β2 κ 为次要尺度参数通常取0或 3−n 。Sigma点集关于均值对称分布精确匹配输入分布的前二阶矩。非线性传播通过逐点映射实现yif(χi),i0,…,2n输出分布统计量通过加权样本重构yˉ∑i02nWi(m)yi,Py∑i02nWi(c)(yi−yˉ)(yi−yˉ)⊤UT变换以 2n1 个采样点捕获非线性变换后分布的均值与协方差精度达三阶矩对于高斯输入显著优于EKF的一阶近似。2.2.2 UKF算法实现标准UKF算法遵循预测-更新框架以UT变换替代线性化。预测步对增广状态向量含过程噪声执行UT变换χk−1a[x^k−1∣k−1⊤,wˉ⊤]⊤,Pk−1∣k−1adiag(Pk−1∣k−1,Qk−1)Sigma点通过非线性状态方程传播χk∣k−1xf(χk−1x,uk,χk−1w)先验估计与协方差通过加权求和获得。更新步类似处理观测方程计算预测观测的Sigma点 Zk∣k−1h(χk∣k−1x) 重构观测均值与协方差进而计算互协方差矩阵 Pxz 与卡尔曼增益等效项KkPxzPzz−1增广UKF处理非加性噪声形式 xkf(xk−1,uk,wk−1) 将噪声维度并入Sigma点采样空间确保噪声非线性耦合的精确传播。状态维度增广至 nq q 为噪声维度计算复杂度 O(L3) 中 Lnq 。迭代UKFIUKF在观测更新中优化Sigma点选择。以最大后验估计为目标x^k∣kargminx[(x−x^k∣k−1)⊤Pk∣k−1−1(x−x^k∣k−1)(zk−h(x))⊤Rk−1(zk−h(x))]Gauss-Newton迭代改善状态估计重新计算线性化点处的Sigma点直至收敛提升强非线性条件下的估计精度。计算复杂度对比显示UKF单次迭代为 O(L3) EKF为 O(Ln2) L 为Sigma点数量。当 n 较小n10 时UKF计算负担与EKF相当高维系统n20 中Sigma点数量 2n1 导致计算成本显著增加但避免了雅可比矩阵推导的软件开发成本。2.2.3 高阶UT变换与变体高阶无迹变换通过扩展Sigma点集匹配更高阶矩。五阶矩匹配采用 n2n1 个点或利用Richardson外推结合不同尺度参数的UT结果提升偏度与峰度估计精度适用于非高斯分布传播。球面单纯形Sigma点集将采样点减至 n2 个保持最小样本量的二阶矩匹配。单纯形顶点位于超球面通过优化算法确定权重分配降低高维UKF计算负担但牺牲对称性可能引入估计偏差。自适应Sigma点策略根据局部线性度动态调整采样密度。估计非线性强度指标ηktr(Py)∥yUT−f(x^)∥ηk 超过阈值时增加Sigma点数量或采用分层采样平衡精度与效率。约束UKF处理状态空间边界如正交性约束、不等式约束。通过投影修正将违规Sigma点映射至可行域或采用约束优化求解加权均值确保估计状态满足物理可行性。2.3 信息滤波与多模型方法2.3.1 信息滤波器Information Filter信息滤波器重构卡尔曼滤波的对偶形式传播信息矩阵 YP−1 与信息向量 y^P−1x^ 。协方差与信息的等价转换通过矩阵求逆实现计算代价 O(n3) 。预测步在信息形式下呈现非线性特性。信息矩阵预测Yk∣k−1[FkYk−1∣k−1−1Fk⊤Qk−1]−1涉及两次矩阵求逆计算复杂度高于协方差形式。但观测更新简化为信息加法Yk∣kYk∣k−1Hk⊤Rk−1Hk,y^k∣ky^k∣k−1Hk⊤Rk−1zk该结构在多传感器融合中极具优势。N 个独立传感器的联合更新Yk∣kYk∣k−1∑i1NHk,i⊤Rk,i−1Hk,i,y^k∣ky^k∣k−1∑i1NHk,i⊤Rk,i−1zk,i各传感器贡献以加法形式融合支持完全分布式架构与异步观测处理。扩展信息滤波EIF将UT变换或线性化应用于信息形式。预测步采用协方差形式的UT变换后转换至信息空间观测更新保持信息加法结构结合UKF的采样精度与信息滤波的融合便利性。2.3.2 多模型估计MMAE/IMM静态多模型方法MMAE并行运行 r 个滤波器各基于不同系统模型 M(j) 。模型似然函数基于新息统计Λk(j)N(y~k(j);0,Sk(j))∣2πSk(j)∣1exp(−21y~k(j)⊤(Sk(j))−1y~k(j))模型概率通过贝叶斯更新μk(j)∑l1rΛk(l)μk−1(l)Λk(j)μk−1(j)最终估计为各滤波器输出的加权平均x^k∣k∑j1rμk(j)x^k∣k(j)交互式多模型IMM引入模型间切换机制。马尔可夫转移矩阵 Π[pij] 描述模型切换概率pijP(Mk(j)∣Mk−1(i)) 。IMM算法周期包含四步1混合概率计算 μk−1(i∣j)pijμk−1(i)/cˉj 2状态混合 x^k−1∣k−1(0j)∑iμk−1(i∣j)x^k−1∣k−1(i) 3并行滤波4模型概率更新与状态组合。混合步骤实现滤波器间的信息交互避免模式切换时的估计跳变。机动目标跟踪是IMM典型应用。模型集包含匀速直线运动CV、匀速转弯CT、匀加速CA等模式。转移矩阵设计反映机动持续性对角元素 0.9∼0.95 非对角元素控制模式切换速率。模型概率演化轨迹提供运动模式识别依据概率阈值 crossing 检测机动起始与终止。