【卡尔曼滤波】第1章 理论基础与标准卡尔曼滤波

第1章 理论基础与标准卡尔曼滤波离散时间动态系统的状态估计理论构成了现代控制工程与信号处理的数学基石。针对线性高斯系统卡尔曼滤波器提供了在最小均方误差准则下的最优递推估计框架。本章从状态空间模型的数学表征出发建立贝叶斯估计的理论基础推导标准卡尔曼滤波算法的完整递推关系分析其数值稳定性问题为后续非线性扩展与高级应用奠定理论根基。1.1 状态空间模型与估计理论基础1.1.1 离散时间状态空间表示考虑一类由随机差分方程描述的离散时间线性系统其动态演化由状态向量 xk​∈Rn 刻画。状态方程表征系统内部动态特性的演化规律xk​Fk​xk−1​Bk​uk​wk−1​其中 Fk​∈Rn×n 表示状态转移矩阵描述系统从时刻 k−1 到 k 的无扰动动态映射Bk​∈Rn×p 为控制输入矩阵uk​∈Rp 表示已知的控制向量。过程噪声 wk−1​∈Rn 建模系统动态中的随机扰动与模型不确定性假设其服从零均值高斯分布 wk−1​∼N(0,Qk−1​) 协方差矩阵 Qk−1​∈Rn×n 对称正定。观测方程建立状态空间与测量空间之间的线性映射关系zk​Hk​xk​vk​观测向量 zk​∈Rm 通过观测矩阵 Hk​∈Rm×n 与状态向量关联。观测噪声 vk​∼N(0,Rk​) 表征传感器测量的随机误差协方差矩阵 Rk​∈Rm×m 反映测量精度。过程噪声与观测噪声统计独立即 E[wi​vj⊤​]0 对所有 i,j 成立。矩阵维度的相容性约束要求状态转移保持维度一致性Fk​ 作用于 n 维状态产生 n 维输出控制矩阵 Bk​ 将 p 维控制输入映射至 n 维状态空间观测矩阵 Hk​ 压缩 n 维状态至 m 维观测空间。通常 m≤n 表明观测数据仅提供状态的部分信息。系统的可观测性决定通过观测序列重构完整状态的能力。构造观测性矩阵Ok​​Hk​Hk1​Fk​⋮Hkn−1​Fkn−2​⋯Fk​​​若 rank(Ok​)n 则系统在时刻 k 完全可观测。PBH秩检验提供了等价的代数判据矩阵对 (F,H) 可观测的充要条件为对所有特征值 λ 有 rank([H⊤,(F⊤−λI)]⊤)n 。可观测性Gramian矩阵 ∑ikkN−1​Φ⊤(i,k)Hi⊤​Ri−1​Hi​Φ(i,k) 的正定性保证有限时间窗口内状态重构的可行性其中 Φ(i,k) 表示状态转移矩阵的链式乘积。1.1.2 贝叶斯估计框架状态估计问题在概率论框架下表述为给定观测历史 z1:k​{z1​,z2​,…,zk​} 对当前状态 xk​ 的统计推断。贝叶斯定理提供后验概率密度函数的递推分解p(xk​∣z1:k​)p(zk​∣z1:k−1​)p(zk​∣xk​)p(xk​∣z1:k−1​)​分子项 p(xk​∣z1:k−1​) 表示先验概率密度融合了过去所有观测信息通过状态转移模型传播至当前时刻p(zk​∣xk​) 为似然函数量化特定状态下获得当前观测的概率分母为归一化常数。该分解体现估计过程的递推特性新观测的引入仅需更新当前先验无需重新处理历史数据。最小均方误差估计MMSE定义后验均值 x^kMMSE​∫xk​p(xk​∣z1:k​)dxk​ 作为最优估计最小化代价函数 JE[(xk​−x^k​)⊤(xk​−x^k​)] 。在高斯分布假设下MMSE估计与最大后验估计MAP等价后者通过最大化后验密度获得。高斯分布在线性变换下保持封闭性若 x∼N(μ,Σ) 则 yAxb∼N(Aμb,AΣA⊤) 。该性质保证线性高斯系统的后验分布始终保持高斯形式完全由均值与协方差表征。估计问题的几何解释在Hilbert空间框架下获得深入理解。将随机变量视为 L2 空间中的向量条件期望对应于观测变量张成子空间上的正交投影。估计误差 x~k​xk​−x^k​ 与观测数据正交即 E[x~k​zi⊤​]0 对所有 i≤k 成立构成正交投影定理的统计表述。1.2 标准卡尔曼滤波算法推导1.2.1 预测步Time Update数学推导预测步基于系统动态方程传播状态估计与误差统计特性。给定 k−1 时刻的后验估计 x^k−1∣k−1​ 与协方差 Pk−1∣k−1​E[(xk−1​−x^k−1∣k−1​)(xk−1​−x^k−1∣k−1​)⊤] 先验估计通过对状态方程取条件期望获得x^k∣k−1​Fk​x^k−1∣k−1​Bk​uk​过程噪声零均值假设消除 wk−1​ 的期望贡献。定义先验估计误差 x~k∣k−1​xk​−x^k∣k−1​ 代入状态方程与预测方程得到误差动态x~k∣k−1​Fk​x~k−1∣k−1​wk−1​先验误差协方差矩阵通过二阶矩计算导出Pk∣k−1​E[x~k∣k−1​x~k∣k−1⊤​]Fk​Pk−1∣k−1​Fk⊤​Qk−1​交叉项消失源于 x~k−1∣k−1​ 与 wk−1​ 的统计独立性。该Lyapunov型方程体现预测过程的不确定性增长第一项传播既往估计误差第二项累加过程噪声引入的新增不确定性。协方差预测的矩阵运算复杂度为 O(n3) 主导计算开销源于密集矩阵的三次乘法。稀疏结构或特定参数化可优化此瓶颈。Joseph形式提供数值稳定性增强的等价表达式Pk∣k−1​(Fk​−Kk​Hk​)Pk−1∣k−1​(Fk​−Kk​Hk​)⊤Kk​Rk​Kk⊤​Qk−1​该形式通过保持对称正定矩阵的求和结构抑制舍入误差导致的非对称性或负定性。1.2.2 更新步Measurement Update数学推导更新步融合新观测数据修正先验估计。定义新息Innovation向量表征观测预测残差y~​k​zk​−Hk​x^k∣k−1​Hk​x~k∣k−1​vk​新息反映实际观测与基于先验估计的预测观测之间的偏差。新息协方差矩阵量化该残差的统计特性Sk​E[y~​k​y~​k⊤​]Hk​Pk∣k−1​Hk⊤​Rk​该矩阵亦称为马氏距离Mahalanobis distance的权重矩阵其逆定义观测空间中的统计距离度量 dk2​y~​k⊤​Sk−1​y~​k​ 用于异常值检测与模型验证。卡尔曼增益矩阵 Kk​∈Rn×m 最小化后验误差协方差矩阵的迹。通过求解 ∂tr(Pk∣k​)/∂Kk​0 获得最优增益Kk​Pk∣k−1​Hk⊤​(Hk​Pk∣k−1​Hk⊤​Rk​)−1Pk∣k−1​Hk⊤​Sk−1​后验估计通过先验估计与新息的线性组合构造x^k∣k​x^k∣k−1​Kk​y~​k​该修正机制中增益矩阵 Kk​ 自动调节观测信息的权重高置信度观测小 Rk​ 对应大增益强先验小 Pk∣k−1​ 对应小增益。后验误差协方差矩阵通过Woodbury矩阵恒等式导出Pk∣k​(I−Kk​Hk​)Pk∣k−1​(I−Kk​Hk​)⊤Kk​Rk​Kk⊤​或等价地Pk∣k​(I−Kk​Hk​)Pk∣k−1​1.2.3 算法性质与稳定性分析卡尔曼滤波器的无偏性保证估计误差零均值特性。通过数学归纳法验证假设 E[x^k−1∣k−1​]E[xk−1​] 则预测步期望 E[x^k∣k−1​]Fk​E[x^k−1∣k−1​]Bk​uk​E[xk​] 更新步中 E[x^k∣k​]E[x^k∣k−1​]Kk​E[y~​k​]E[xk​] 因 E[y~​k​]0 。归纳基础由初始条件 E[\hat{\mathbf{x}}_{0|0}] E[\mathbf{x}}_0] 确立。最优性体现于估计误差与新息序列的白噪声性质。正交性原理保证 E[x~k∣k​y~​i⊤​]0 对所有 i≤k 成立表明当前估计已充分利用所有历史观测信息。新息序列 {y~​k​} 构成白噪声过程其协方差 E[y~​k​y~​j⊤​]Sk​δkj​ 该性质可用于自适应噪声估计与故障诊断。误差协方差矩阵的演化服从离散时间Riccati方程Pk∣k​Fk​Pk−1∣k−1​Fk⊤​Qk−1​−Fk​Pk−1∣k−1​Hk⊤​(Hk​Pk−1∣k−1​Hk⊤​Rk​)−1Hk​Pk−1∣k−1​Fk⊤​在定常系统 (F,H,Q,R) 条件下若 (F,H) 可观测且 (F,Q1/2) 可控则 Pk∣k​ 指数收敛至唯一正定稳态解 P∞​ 满足代数Riccati方程。稳态卡尔曼增益 K∞​P∞​H⊤(HP∞​H⊤R)−1 支持次优但计算高效的稳态滤波器实现。对偶性理论建立卡尔曼滤波与线性二次调节器LQR的数学同构。估计问题的Riccati方程对应控制问题的Riccati方程其中时间方向倒置B 替代 H⊤ Q 与 R 互换角色。该对偶性暗示估计器与观测器具有相同的稳定性条件。1.3 基础实现与数值问题1.3.1 Python/MATLAB标准实现算法实现采用面向对象架构封装状态估计逻辑。核心类结构包含初始化方法配置系统矩阵与噪声参数预测方法执行时间更新更新方法处理观测数据。向量化运算避免显式Python循环利用NumPy的BLAS/LAPACK后端优化矩阵运算性能。批量处理模式适用于离线数据分析存储完整观测序列后一次性执行前向滤波实时流式处理模式针对在线应用每接收新观测立即执行单次预测-更新循环维护当前状态估计与协方差。两种模式在数学等价性基础上针对内存与延迟约束优化实现。验证框架采用pytest构建单元测试比较实现输出与参考解或解析解的一致性。数值梯度检验验证雅可比矩阵计算蒙特卡洛仿真评估估计器的统计特性无偏性、一致性是否满足理论预期。1.3.2 数值稳定性处理有限精度算术引入舍入误差累积可能导致协方差矩阵失去对称正定性。强制对称化操作 (PP⊤)/2 在每次协方差更新后执行消除数值不对称。条件数监控 κ(P)∥P∥∥P−1∥ 检测矩阵病态触发警告或采用高精度运算。平方根滤波Square-Root Filtering通过传播协方差矩阵的Cholesky因子 PSS⊤ 而非矩阵本身将动态范围缩减一半改善数值条件。Potter算法针对观测更新设计Bierman的UD分解将协方差表示为 PUDU⊤ U 单位上三角D 对角避免Cholesky分解的开方运算适用于嵌入式系统的定点实现。双精度float64与单精度float32运算的精度边界取决于问题条件数与观测序列长度。对于高维状态或长时间运行推荐采用双精度存储关键协方差变量或周期性执行正交变换重置数值误差累积。以上内容为第一章的完整