卡尔曼滤波专题培训课件
合集下载
《卡尔曼滤波教学》PPT课件
AS ˆ((k k 1 ) )H(K C )[(X k S ˆ(()k k A 1))(]
(6-61) 由（6－56）～（6－61）可以画出卡尔 曼滤波对 S (k )进行估计的递推模型，如 图6.13所示
• 输入为观测值X(k)，输出为信号估计 值 Sˆ (k) 。
X(k) X~(k) H(k)
X ~(k)X(k X ˆ()k) (6-60)
显然，新息的产生是由于我们前面忽略 了w1(k)与 w(k)所引起的
• 用新息X~(k)乘以一个修正矩阵 H(k )，用 它来代替式（6－56）的w1(k来) 对S (k )进 行估计：
S ˆ(k A )S ˆ( k 1 )H ) X ~ ((k k))
令 Cε ((kk))τ C R (k (k S ) τ )S ，Uε(k)C(kτ ) 代入上式化简：
ε(k ) ε(k H ) (τK U)H τ U H (k (τ ) H k)τ(S kS
ε ( k U )τ ) ( 1 U S τ [S H U (τ ) k 1 ( ]S ) [S H U τ ( ) 1 ( ] k τS
Xˆ (k) C(k)
Sˆ (k)
z 1
A(k ) Sˆ (k 1)
图6.13 卡尔曼滤波的一步递推法模型
６.２.２ 卡尔曼滤波的递推公式 从图6.13容易看出，要估计出 Sˆ (k) 就必须 要先找到最小均方误差下的修正矩阵
H (k )，结合式（6－61）、（6－56）、 （6－57）得：
S ˆ(k A )S ˆ( ( k k 1 H ) ) (K (k w )) [ ( C C k()S ˆ k ( k ) 1S A )
z w1(k ) S(k1) 1
S (k ) C(k)
(6-61) 由（6－56）～（6－61）可以画出卡尔 曼滤波对 S (k )进行估计的递推模型，如 图6.13所示
• 输入为观测值X(k)，输出为信号估计 值 Sˆ (k) 。
X(k) X~(k) H(k)
X ~(k)X(k X ˆ()k) (6-60)
显然，新息的产生是由于我们前面忽略 了w1(k)与 w(k)所引起的
• 用新息X~(k)乘以一个修正矩阵 H(k )，用 它来代替式（6－56）的w1(k来) 对S (k )进 行估计：
S ˆ(k A )S ˆ( k 1 )H ) X ~ ((k k))
令 Cε ((kk))τ C R (k (k S ) τ )S ，Uε(k)C(kτ ) 代入上式化简：
ε(k ) ε(k H ) (τK U)H τ U H (k (τ ) H k)τ(S kS
ε ( k U )τ ) ( 1 U S τ [S H U (τ ) k 1 ( ]S ) [S H U τ ( ) 1 ( ] k τS
Xˆ (k) C(k)
Sˆ (k)
z 1
A(k ) Sˆ (k 1)
图6.13 卡尔曼滤波的一步递推法模型
６.２.２ 卡尔曼滤波的递推公式 从图6.13容易看出，要估计出 Sˆ (k) 就必须 要先找到最小均方误差下的修正矩阵
H (k )，结合式（6－61）、（6－56）、 （6－57）得：
S ˆ(k A )S ˆ( ( k k 1 H ) ) (K (k w )) [ ( C C k()S ˆ k ( k ) 1S A )
z w1(k ) S(k1) 1
S (k ) C(k)
卡尔曼滤波方法资料课件
采用最小均方误差准则，通过最小化估计误 差的平方和实现状态估计。
线性最小方差估计方法的优 点
适用于线性系统状态估计，计算量较小，易于实现。
线性最小方差估计方法的 缺点
对非线性系统效果不佳，需要先验知识或模 型参数。
04
卡尔曼滤波方法的实现 和应用案例
卡尔曼滤波方法的软件实现
软件平台
可以使用Python、C、Matlab等编程语言实现卡尔曼滤波算法。
卡尔曼滤波方法在控制系统中的应用案例
应用场景
卡尔曼滤波方法在控制系统中主要用于估计系统的状态变量。
案例分析
通过实际控制系统的数据和实验，验证卡尔曼滤波方法在控制系统中的可行性和稳定性。
卡尔曼滤波方法在雷达系统中的应用案例
应用场景
卡尔曼滤波方法在雷达系统中主要用于 目标跟踪和运动参数估计。
VS
案例分析
卡尔曼滤波方法的基本概念和原理
基本概念
卡尔曼滤波方法是一种递归估计方法，通过建立状态方程和观测方程，对系统状态进行最优估计。
原理
卡尔曼滤波方法基于最小均方误差准则，通过不断更新估计值来逼近真实值，具有计算量小、实时性 强的优点。
卡尔曼滤波方法的应用领域
机器人
用于机器人的定位、路径规划、 避障等。
描述系统状态和观测之间的关系。
定义初始状态和误差协方差
02
确定系统初始状态和误差协方差的估计值，为后续的滤波过程
提供初始条件。
选择合适的模型参数
03
根据实际情况选择合适的模型参数，如系统动态参数、观测参
数等，以更好地描述系统特性。
预测步骤
01
根据上一时刻的状态和误差协方 差，预测当前时刻的系统状态和 误差协方差。
线性最小方差估计方法的优 点
适用于线性系统状态估计，计算量较小，易于实现。
线性最小方差估计方法的 缺点
对非线性系统效果不佳，需要先验知识或模 型参数。
04
卡尔曼滤波方法的实现 和应用案例
卡尔曼滤波方法的软件实现
软件平台
可以使用Python、C、Matlab等编程语言实现卡尔曼滤波算法。
卡尔曼滤波方法在控制系统中的应用案例
应用场景
卡尔曼滤波方法在控制系统中主要用于估计系统的状态变量。
案例分析
通过实际控制系统的数据和实验，验证卡尔曼滤波方法在控制系统中的可行性和稳定性。
卡尔曼滤波方法在雷达系统中的应用案例
应用场景
卡尔曼滤波方法在雷达系统中主要用于 目标跟踪和运动参数估计。
VS
案例分析
卡尔曼滤波方法的基本概念和原理
基本概念
卡尔曼滤波方法是一种递归估计方法，通过建立状态方程和观测方程，对系统状态进行最优估计。
原理
卡尔曼滤波方法基于最小均方误差准则，通过不断更新估计值来逼近真实值，具有计算量小、实时性 强的优点。
卡尔曼滤波方法的应用领域
机器人
用于机器人的定位、路径规划、 避障等。
描述系统状态和观测之间的关系。
定义初始状态和误差协方差
02
确定系统初始状态和误差协方差的估计值，为后续的滤波过程
提供初始条件。
选择合适的模型参数
03
根据实际情况选择合适的模型参数，如系统动态参数、观测参
数等，以更好地描述系统特性。
预测步骤
01
根据上一时刻的状态和误差协方 差，预测当前时刻的系统状态和 误差协方差。
卡尔曼滤波介绍ppt课件(共29张PPT)
卡尔曼滤波是一种利用目标动态信息去除噪声影响，得到目标位置良好估计的方法。它适用于雷达跟踪等场景，其中目标位置、速度、加速度的测量值常含有噪声。通过贝叶斯理论推导，卡尔曼滤波能够实现对当前、未来或过去位置的估计，分别对应滤波、预测和插值或平滑操作。典型实例包括从有限且包含噪声的观察序列中预测物体位置坐标及速度。此外，文档还探讨了扩展卡尔曼滤波（EKF）的推导过程，并展示了其在无人车定位等实际应用中的价值。紧组，突显了卡尔曼滤波在处理有色噪声及提高导航系统性能方面的重要性。
中文第二章卡尔曼滤波器-课件
式
f a1cG
s ˆ n n a s ˆ n 1 n 1 G n x n a s ˆ n 1 c n 1
新息
一步预测: a s ˆn 1 n 1 s ˆn n 1
第二步预测: x ˆ n n 1 c s ˆ n n 1 a s ˆ n c 1 n 1
zk hkxk,nk
pzk xk k0
v和n分别为方差为Q和R的高斯白噪声 需要注意的是：这里x表示信号状态，z表示观察/测量值。
贝叶斯估计
假设需要计算的后验分布 pxk1|z1:k1在时刻k-1已经得到，那
么我们利用状态模型可以获得时刻k状态的先验概率分布：
p x k |z 1 : k 1 p x k |x k 1p x k 1 |z 1 : k 1 d x k 1
卡尔曼滤波器认为后验概率在任何时刻都是高斯分布的，这样由均
值和方差就可以完全确定其概率分布。可以证明，如果 pxk1|z1:k1 是高 斯的，那么要使 pxk |z1:k 也是高斯的话，隐含了下面的假设：
v和n都是参数已知的高斯分布
fk xk1,vk 是 x k 1 和 v k 的线性函数 hk xk,nk 是 x k 和n k 的线性函数
nE e2n Een sns ˆnn Eensn
PncG nPn
1cG nPnPn ensnsˆnn 估计误差
结构框图
计算步骤
P na2 n 1 Q
Gn RccP2nPn
n 1 cn G P n
s ˆ n n a s ˆ n 1 n 1 G n x n a s ˆ n 1 c n 1
新息（Innovation）: n x n x ˆ n n 1 x n a s ˆ n 1 c n 1
卡尔曼滤波方法PPT课件
17
第17页/共28页
联邦滤波器算法
• 信息分配
在进入下一次递推之前，需将主滤波器中的信息 （状态、方差）在各子滤波器中按如下规则进行分配：
N
Xˆ i Xˆ g ,
Pii
P 1
ig
,
Q1
Qi1 Qm1
i 1
其中，Qi m1Q , i , i 1,, N, m 为信息分配系数，m 为
主滤波器的信息分配系数，满足守恒原则
方差估值 Pk k [I Kk Hk ]Pk k1
6
第6页/共28页
3.5 卡尔曼滤波的结构图
上述递推公式，称为卡尔曼滤波器。实际上，卡尔曼 滤波器也是一个系统，其结构框图如下：
Zk + -
+
Kk
+
Z k|k 1
当前估计值
Xˆ k
延时 一步
Hk
k ,k 1
一步预测
上一步估计值
Xˆ k|k 1
第27页/共28页
感谢您的观看！
28
第28页/共28页
Yi f ( i )
24
第24页/共28页
Unscented卡尔曼滤波(续) 变换样本点Yi 即可近似表示 y 的分布。下面利用 Yi 来计算 y 的均值和方差。
3. 计算 y 的均值和方差
p
y Wi(m)Yi
i0
p
Py Wi(c) (Yi y)(Yi y)T i0
其中，
Wi(m)
Wi(c)
得预测测量估计偏差： Z~k|k1 Zk Zˆk|k1 Zk Hk Xˆ k|k1
利用此偏差修正预测估计：
Xˆ k|k Xˆ k k1 Kk [Zk Hk Xˆ k k1]
第17页/共28页
联邦滤波器算法
• 信息分配
在进入下一次递推之前，需将主滤波器中的信息 （状态、方差）在各子滤波器中按如下规则进行分配：
N
Xˆ i Xˆ g ,
Pii
P 1
ig
,
Q1
Qi1 Qm1
i 1
其中，Qi m1Q , i , i 1,, N, m 为信息分配系数，m 为
主滤波器的信息分配系数，满足守恒原则
方差估值 Pk k [I Kk Hk ]Pk k1
6
第6页/共28页
3.5 卡尔曼滤波的结构图
上述递推公式，称为卡尔曼滤波器。实际上，卡尔曼 滤波器也是一个系统，其结构框图如下：
Zk + -
+
Kk
+
Z k|k 1
当前估计值
Xˆ k
延时 一步
Hk
k ,k 1
一步预测
上一步估计值
Xˆ k|k 1
第27页/共28页
感谢您的观看！
28
第28页/共28页
Yi f ( i )
24
第24页/共28页
Unscented卡尔曼滤波(续) 变换样本点Yi 即可近似表示 y 的分布。下面利用 Yi 来计算 y 的均值和方差。
3. 计算 y 的均值和方差
p
y Wi(m)Yi
i0
p
Py Wi(c) (Yi y)(Yi y)T i0
其中，
Wi(m)
Wi(c)
得预测测量估计偏差： Z~k|k1 Zk Zˆk|k1 Zk Hk Xˆ k|k1
利用此偏差修正预测估计：
Xˆ k|k Xˆ k k1 Kk [Zk Hk Xˆ k k1]
卡尔曼滤波器分类及基本公式概要课件
精确地描述系统的非线性特性。
无迹卡尔曼滤波器的计算较为复杂，但具有更高的估计精度和
03
稳定性，适用于一些高精度要求的非线性系统状态估计。
03
卡尔曼滤波器的基本公 式
状态方程
描述系统状态变化的数学表达式。
状态方程是描述系统状态变化的数学表达式，它基于系统的动态模型和当前状态 ，计算未来状态。在卡尔曼滤波器中，状态方程用于预测系统的下一个状态。
详细描述
卡尔曼增益矩阵的计算基于状态向量和误差 协方差矩阵，通过一系列数学运算得到。它 反映了新获取的测量值对状态估计的贡献程 度，以及旧信息的保留程度。在计算过程中 ，通常采用递推或迭代的方式进行计算，以 降低计算复杂度。
更新状态向量和误差协方差矩阵
总结词
在得到卡尔曼增益矩阵后，需要利用它来更 新状态向量和误差协方差矩阵，以完成一次 滤波过程。0203 Nhomakorabea改进
针对不同应用场景和需求，卡尔曼滤 波器不断有新的改进和优化算法出现 。
滤波器的应用领域
航空航天
卡尔曼滤波器在航空航天领域 中用于导航、姿态估计和卫星
轨道计算等。
无人驾驶
卡尔曼滤波器在无人驾驶汽车 中用于传感器数据处理、路径 规划和障碍物检测等。
机器人
卡尔曼滤波器在机器人领域中 用于定位、地图构建和姿态控 制等。
02
扩展卡尔曼滤波器通过将非线性函数进行线性化处 理，将非线性问题转化为线性问题进行解决。
03
扩展卡尔曼滤波器的计算相对复杂，但适用范围较 广，适用于大多数非线性系统的状态估计。
无迹卡尔曼滤波器
01
无迹卡尔曼滤波器是另一种针对非线性系统的改进型卡尔曼滤 波器。
02
无迹卡尔曼滤波器采用无迹变换方法处理非线性函数，能够更
Kalman滤波简介ppt课件
2021/4/26
精选2021版alman滤波是一种实时递推算法，它所处理的是随机信号， 利用系统噪声和观测噪声的统计特性，以系统的观测量作为滤 波器的输入，以所要估计值（状态或参数）作为滤波器的输出 ，滤波器输入与输出是由时间更新和观测更新算法联系在一起 的，根据系统方程和观测方程估计出所需要处理的信号——实 质是一种最优估计方法。 卡尔曼滤波就是在有随机干扰和噪声的情况下，以线性最小方 差估计方法给出状态的最优估计值，卡尔曼滤波是在统计的意 义上给出最接近状态真值的估计值。
2021/4/26
精选2021版课件
10
随机信号没有确定的频谱.无法用常规滤波提取或抑制信号.但
随机信号具有确定的功率谱，所以可根据有用信号和干扰信 号的功率谱设计滤波器。维纳滤波是解决此类问题的方法之一 。但设计维纳滤波器须作功率谱分解，只有当被处理信号为平 稳的，干扰信号和有用信号均为一维，且功率谱为有理分式时 ，维纳滤波器的传递函数才可用伯特一香农设计法较容易地求
2021/4/26
精选2021版课件
4
Kalman滤波控制系统结构图
由于系统的状态x是不确定的，卡尔曼滤波器的任 务就是在有随机干扰w和噪声v的情况下给出系统状态x
的最优估算值 xˆ ，它在统计意义下最接近状态的真值x ，从而实现最优控制u( xˆ)的目的。
2021/4/26
精选2021版课件
5
Use For
解出。否则设计维纳滤波器存在着诸多困难。维纳滤波除设
计思想与常规滤波不同外.对信号作抑制和选通这一点是相似 的。
2021/4/26
精选2021版课件
11
卡尔曼滤波从与被提取信号有关的量测量中通过算法估计出
所需信号。其中被估计信号是由白噪声激励引起的随机响应 ，激励源与响应之问的传递结构(系统方程)已知.量测量与被 估计量之间的函数关系(量测方程)也已知。估计过程中利用 了如下信息:系统方程、量测方程、白噪声激励的统计特性、 量测误差的统计特性。由于所用信息都是时域内的量。所以
《卡尔曼滤波》课件
3
无迹卡尔曼滤波线性系统的 估计。
卡尔曼滤波的应用案例
飞行器姿态估计
卡尔曼滤波在航空领域中被广泛应用于飞行器姿态估计，用于提高飞行器的稳定性和导航准 确性。
目标跟踪
卡尔曼滤波可用于跟踪移动目标的位置和速度，常见于机器人导航和视频监控等领域。
3 卡尔曼滤波的应用领
域
卡尔曼滤波被广泛应用于 航空航天、机器人、金融 等领域，用于提高系统的 状态估计精度。
卡尔曼滤波的数学模型
状态空间模型
卡尔曼滤波使用状态 空间模型表示系统的 状态和观测值之间的 关系，包括状态方程 和测量方程。
测量方程
测量方程描述观测值 与系统状态之间的关 系，用于将观测值纳 入到状态估计中。
了解更多关于卡尔曼滤波的内容和应用，推荐文献、学术论文和在线课程等资源。
《卡尔曼滤波》PPT课件
卡尔曼滤波是一种优秀的状态估计方法，被广泛用于目标跟踪、姿态估计和 股票预测等领域。
介绍卡尔曼滤波
1 什么是卡尔曼滤波？
卡尔曼滤波是一种递归状 态估计算法，用于通过系 统模型和测量信息估计系 统状态。
2 卡尔曼滤波的基本原
理
卡尔曼滤波基于贝叶斯估 计理论，通过最小化估计 误差的均方差来优化状态 估计。
股票预测
卡尔曼滤波可以应用于股票市场，通过对历史数据进行分析和预测，提供股票价格的预测和 趋势分析。
卡尔曼滤波的优化算法
粒子滤波
粒子滤波是一种基于蒙特卡洛 方法的状态估计算法，适用于 非线性和非高斯系统，提供更 广泛的估计能力。
自适应滤波
自适应滤波是一种根据系统的 特点自动调整滤波参数的方法， 提供更好的适应性和鲁棒性。
非线性滤波
非线性滤波是对卡尔曼滤波算 法的改进，用于处理非线性系 统和测量模型，提供更准确的 状态估计。
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
1组观测向量
是一组针对同一时刻的系统特性的测量值，例如观测量可以包括GNSS 系统的位置测量值，或者INS与GNSS位置结果的差值。
1个算法：
卡尔曼滤波算法 使用观测向量、观测模型和系统模型来获得状态向量的最优估计，分为
系统传递和测量更新两个部分。
平滑算法
10
目 录 1.5 卡尔曼滤波的导航应用
• 概述 • 经典KF • EKF • LKF
惯性导航系统（INS）的精对准和标定 单一导航（GNSS, 无线电、水声学、匹配） 组合导航
INS/GNSS组合导航及多传感器组合导航 INS/水声组合导航 INS/匹配导航
…
11
二、Kalman滤波
2019/12/19
12
2.1 卡尔曼滤波方程
平滑算法
7
目录
概述 标准 KF 扩展 KF Schmidt KF 自适应 KF 平滑算法
状态向量（状态） 是一组描述系统的参数。 可以是常量，也可是时变量，是估计对象。 与之相关联的是误差协方差矩阵，描述了状态估计的不确定
度及估计误差间的相关度。
8
1.4 卡尔曼滤波的要素
目录
4个要素：2个模型、1组观测量、1个算法
Schmidt KF
自适应 KF
平滑算法
5
目录
• 概述 标准 KF 扩展 KF Schmidt KF
Kalman滤波是一种递推线性最小方差估计
在提供的初始估计基础上，卡尔曼滤波通过递归运算，用先验值和最 新观测数据的加权平均来更新状态估计（老息+新息）。
非递归算法（如标准最小二乘）中没有先验估计，估计结果由全部观 测数据计算而来（新息） 。
目 录 1. 离散系统的数学描述
设离散化后的系统状态方程和量测方程分别为：
概述 标准 KF 扩展 KF
Xk
X k,k 1 k 1
k 1Wk 1
Zk Hk Xk Vk
Schmidt KF
自适应 KF 平滑算法
Xk为k时k-1刻到的k时n维刻状的态Γ系Wk向-统1k为-量1一为系步k统-1状时噪态刻声的矩系阵统噪声 Zk为（k被H时转k估刻为移计的k矩时量mV阵刻k维）为（系量kn时统测×（刻量向nn阶×m测量维）r矩阶（量阵）r维测）噪声 （m×n阶）
15
目 录 2. 离散卡尔曼滤波方程
状态一步预测方程
概述
状态估值计算方程
标准 KF
滤波增益方程
扩展 KF
一步预测均方差方程
Schmidt KF
自适应 KF 估计均方差方程
平滑算法
Xˆ k/k- 1 = k,k- 1Xˆ k- 1 Xˆ k Xˆ k /k 1 K k (Zk
16
目录
概述 标准 KF 扩展 KF Schmidt KF 自适应 KF 平滑算法
时间更新 方程
量测修正 方程
Xˆ k /k 1 k,k 1Xˆ k 1
H k Xˆ k /k 1 )
Kk
Pk/k
1
H
T k
(H
k
Pk
/
k
1H
T k
Rk) 1
Pk/k- 1
=
k,k-
1Pk-
T
1 k,k-
1
+
Gk- 1Qk- 1GTk- 1
Pk
(I K k H k )Pk/k 1 (I K k H k )T
K
k
R
k
K
T k
或 Pk (I K k Hk )Pk/k 1
0 (k j) kj 1 (k j)
14
目录
概述 标准 KF 扩展 KF Schmidt KF 自适应 KF 平滑算法
初始状态的一、二阶统计特性为：
E X0 mx0
Var X0 Cx0
Var{·} 为对{·}求方差的符号
卡尔曼滤波要求mx0和Cx0为已知量，
且要求X0与{Wk}和{Vk}都不相关
：卡尔曼滤波
目录
一. 概述
二. 标准卡尔曼滤波
卡尔曼滤波方程
闭环卡尔曼滤波
卡尔曼滤波特性及实现中的问题
三. 扩展卡尔曼滤波
非线性系统
线性化卡尔曼滤波
扩展卡尔曼滤波
四. Schmidt 卡尔曼滤波
五. 自适应卡尔曼滤波
2
一、概述Biblioteka 2019/12/193
目 录 1.1 Rudolf Emil Kalman
4
1.2 概述
目录
• 概述 标准 KF
Kalman滤波是一种最优估计算法，而非滤波器
能够实时估计系统中的参数（如连续变化的位置、速度等信息）。 估计量通过一系列受噪声污染的观测量来更新， 观测量必须是待估参数的函数，但是在给定的时刻，不要求观测量能
够唯一确定当时的参数值。
扩展 KF
概述 2个模型
标准 KF 扩展 KF Schmidt KF 自适应 KF 平滑算法
系统模型
也称过程模型或者时间传递模型，描述了状态与误差协方差 矩阵随时间的变化特性。
对于选定状态量，系统模型是确定的。
观测模型
描述了观测向量与状态向量间的函数关系。
9
目录 概述
标准 KF 扩展 KF Schmidt KF 自适应 KF
• 概述 标准 KF 扩展 KF Schmidt KF
Born 1930 in Hungary BS and MS from MIT PhD 1957 from Columbia Filter developed in 1960-61
自适应 KF
平滑算法
Kalman R E. A new approach to linear filtering and prediction problems [J]. Journal of Fluids Engineering, 1960, 82(1): 35-45. （引用：18083）
最小方差估计
线性最小方差估计
递推线性最小 方差估计
自适应 KF 平滑算法
卡尔曼滤波是一种贝叶斯估计
6
目录
1.3 卡尔曼滤波的要素和流程
实际系统
概述 标准 KF
系统模型
观测模型
观测向量及 其协方差
状态向量及 其协方差
扩展 KF Schmidt KF
卡尔曼滤波算法
自适应 KF （实线表示数据流一直有，虚线表示只在某些应用中有，Ref：Paul Groves）
13
目 录 要求{Wk}和{Vk}是互不相关的、零均值白噪声序列：
概述 标准 KF
E Wk WjT E Vk VjT
Qk kj R k kj
扩展 KF Qk和Rk分别称为系统噪声和量测噪声的方差矩阵，分别 Schmidt KF 是已知值的非负定阵和正定阵；
自适应 KF 平滑算法
δk j 是Kronecker δ函数，即：