扩展Kalman滤波(EKF)和无迹卡尔曼滤波(ukf)分析
- 格式：ppt
- 大小：1.08 MB
- 文档页数：29
文档推荐
-
扩展卡尔曼滤波算法的matlab程序页数:6
-
扩展卡尔曼滤波matlab程序页数:3
-
扩展卡尔曼滤波的影响因素分析_王京伟页数:3
-
扩展卡尔曼滤波原理及其应用页数:15
-
扩展卡尔曼滤波原理及其应用页数:15
下载文档原格式
合集下载
非线性卡尔曼滤波器
● UT变换的特点是对非线性函数的概率密度分布进行近似，而不是对非线性函数进行近似，即使系统模 型复杂，也不增加算法实现的难度；而且所得到的非线性函数的统计量的准确性可以达到三阶；除此 之外，它不需要计算雅可比矩阵，可以处理不可导非线性函数。
UKF计算步骤：
PF
PF
● 粒子滤波(PF: Particle Filter)的思想基于蒙特卡洛方法(Monte Carlo methods)，它是 利用粒子集来表示概率，可以用在任何形式的状态空间模型上。其核心思想是通过从后 验概率中抽取的随机状态粒子来表达其分布，是一种顺序重要性采样法(Sequential Importance Sampling)。简单来说，粒子滤波法是指通过寻找一组在状态空间传播的随 机样本对概率密度函数进行近似，以样本均值代替积分运算，从而获得状态最小方差分 布的过程。这里的样本即指粒子,当样本数量N→∝时可以逼近任何形式的概率密度分布。
EKF
首先围绕滤波值 xˆk 将非线性函数 f , g 展开Taylor级数并
略去二阶及以上项，得到一个近似的线性化模型，然后应用 Kalman滤波完成对其目标的滤波估计等处理。
1.对状态模型的一阶Taylor展示：
xk f
xˆk 1
f xˆk 1
xk 1 xˆk 1 k
令
f
F
xˆk 1
f1 f1
xˆ1
xˆ2
f xˆk 1
F
f2 xˆ1
f2 xˆ2
fn fn
xˆ1 xˆ2
f1
xˆn
f2 xˆn
fn
xˆn
g1 g1
xˆ1
xˆ2
g xˆk
H
g2 xˆ1
g2 xˆ2
UKF计算步骤：
PF
PF
● 粒子滤波(PF: Particle Filter)的思想基于蒙特卡洛方法(Monte Carlo methods)，它是 利用粒子集来表示概率，可以用在任何形式的状态空间模型上。其核心思想是通过从后 验概率中抽取的随机状态粒子来表达其分布，是一种顺序重要性采样法(Sequential Importance Sampling)。简单来说，粒子滤波法是指通过寻找一组在状态空间传播的随 机样本对概率密度函数进行近似，以样本均值代替积分运算，从而获得状态最小方差分 布的过程。这里的样本即指粒子,当样本数量N→∝时可以逼近任何形式的概率密度分布。
EKF
首先围绕滤波值 xˆk 将非线性函数 f , g 展开Taylor级数并
略去二阶及以上项，得到一个近似的线性化模型，然后应用 Kalman滤波完成对其目标的滤波估计等处理。
1.对状态模型的一阶Taylor展示：
xk f
xˆk 1
f xˆk 1
xk 1 xˆk 1 k
令
f
F
xˆk 1
f1 f1
xˆ1
xˆ2
f xˆk 1
F
f2 xˆ1
f2 xˆ2
fn fn
xˆ1 xˆ2
f1
xˆn
f2 xˆn
fn
xˆn
g1 g1
xˆ1
xˆ2
g xˆk
H
g2 xˆ1
g2 xˆ2
EKFUKFPF算法的比较程序
EKFUKFPF算法的比较程序在估计理论中，EKF（Extended Kalman Filter），UKF（Unscented Kalman Filter）和PF（Particle Filter）是三种常用的非线性滤波算法。
它们在不同的环境和应用中具有不同的优点和缺点。
下面将对这三种算法进行比较。
首先，EKF是最常用的非线性滤波算法之一、它通过线性化状态转移方程和测量方程来近似非线性问题。
EKF在处理高斯噪声的情况下表现良好，但在处理非高斯噪声时会有较大的误差。
由于线性化过程的存在，EKF对于高度非线性和非高斯问题可能表现不佳。
此外，EKF对系统模型的准确性要求较高，较大的模型误差可能导致滤波结果的不准确性。
其次，UKF通过构造一组代表系统状态的Sigma点，通过非线性映射来近似非线性函数。
相较于EKF，UKF无需线性化系统模型，因此适用于更广泛的非线性系统。
UKF的优点是相对较好地处理了非线性系统和非高斯噪声，但在处理维数较高的问题时，计算开销较大。
最后，PF是一种基于粒子的滤波方法，通过使用一组代表系统状态的粒子来近似概率密度函数。
PF的优点是它可以处理非线性系统和非高斯噪声，并且在系统模型不准确或缺乏确定性时，具有较好的鲁棒性。
由于粒子的数量可以灵活调整，PF可以提供较高的估计精度。
然而，PF的计算开销较大，尤其在高维度的情况下。
综上所述，EKF、UKF和PF是三种常用的非线性滤波算法。
EKF适用于高斯噪声条件下的非线性问题，但对系统模型准确性要求高。
UKF适用于一般的非线性问题，但计算开销较大。
PF适用于非线性和非高斯噪声条件下的问题，并具有较好的鲁棒性，但在计算开销方面具有一定的挑战。
在实际应用中，我们应根据具体问题的性质和要求选择合适的算法。
比如，在低维情况下，EKF是一个可行的选择；在高维或非高斯噪声情况下，可以考虑使用UKF或PF算法。
EKF、UKF和CKF的滤波性能对比研究
X ( k + 1)-0 ( k + 1 |k)X ( k) +G ( k)W ( k) +0 ( k)
(5)
式中，初始值为X(0) - E[X(0)。
综上所述，该线性化过程同普通卡尔曼滤波相比，在前一步滤波值X ( k)已经得到的条件下，状态方
程(5)增加了方程之外作用的项0( k)。
在式(1)中系统状态方程的基础上，再将非线性函数h ( * )围绕X (k)展开成一阶Taylor级数，得
+ G[X(T) ,kjw(T)
(2)
式中，@( *)为非线性函数;X ( k)为滤波值。 令
2@ -9fLX ( k),k] 9X( k) 2X ( k)
=0 ( k + 1 | k)
X ( k)-X ( k)
(3)
)( f
k"2h)Z：X k) - X k) X ( k)-0 ( k)
(4)
则状态方程为
最近，基于Cubature变换的CKF滤波算法由Arasaratnam et al提出[34]，该算法在状态估计、机动 目标定位等领域得到广泛的应用。CKF应用球面径向准则对状态后验均值和协方差进行逼近，并通过 2”个相同权值的Cubature点经系统方程得到新的状态点进而推导出下一时刻状态的预测点，整个过程 不需对复杂的Jacobi矩阵进行计算，具有很好的滤波效果⑹。针对目前的相关文献，大多数是基于EKF 与UKF滤波算法的应用对比研究 ，以及UKF与CKF滤波算法的应用对比研究 ，而基于3种滤波算法滤 波性能对比研究的文献尚不存在。因此本文在相同的非线性系统模型下，对比分析了 EKF、UKF和CKF 滤波算法的实现原理以及各自估计值与真实值的误差，综合比较了 3种算法的滤波性能。
扩展Kalman滤波(EKF)和无迹卡尔曼滤波(ukf).共31页
谢谢！
51、 天 下 之 事 常成 于困约 ，而败 于奢靡 。——陆 游 52、 生 命 不 等 于是呼 吸，生 命是活 动。——卢 梭
53、 伟 大 的 事 业，需 要决心 ，能力 ，组织 和责任 感。 ——易 卜 生 54、 唯 书 籍 不 朽。——乔 特
55、 为 中 华 之 崛起而 读书。 ——周 恩来
扩展Kalman滤波(EKF)和无迹卡尔曼 滤波(ukf).
11、用道德的示范来造就一个人，显然比用法律来约束他更有价值。—— 希腊
12、法律是无私的，对谁都一视同仁。在每件事制不了好的自由，因为好人不会去做法律不允许的事 情。——弗劳德
14、法律是为了保护无辜而制定的。——爱略特 15、像房子一样，法律和法律都是相互依存的。——伯克
扩展卡尔曼滤波算法讲解
扩展卡尔曼滤波算法讲解
扩展卡尔曼滤波算法（ExtendedKalmanFilter，简称EKF）是一种常用于非线性系统状态估计的算法。
它是基于卡尔曼滤波算法的扩展，利用泰勒级数对非线性系统进行线性化，使得卡尔曼滤波能够处理非线性系统。
EKF算法在航空、导航、控制等领域中得到了广泛的应用。
EKF算法的基本原理是通过对系统状态进行预测和更新，来估计系统的状态。
预测阶段利用系统的动力学方程，对系统状态进行预测；更新阶段利用系统的观测方程，将观测值与预测值进行比较，从而得到最优的状态估计值。
EKF算法的关键在于如何进行状态线性化。
通常采用泰勒级数对非线性函数进行近似，得到局部线性化的系统模型。
同时，为了最大限度地减小线性化误差，需要在每次更新时重新计算线性化矩阵。
EKF算法的优点在于能够适用于很多非线性系统，且具有较高的精度和稳定性。
缺点在于计算量较大，实时性较差。
为了解决这个问题，可以采用一些优化算法，如增量式EKF、UKF等，来提高计算速度和精度。
总之，EKF算法是一种非常实用的状态估计算法，能够有效地处理非线性系统。
在实际应用中，需要根据具体情况进行调整和优化，以达到最优的效果。
- 1 -。
拓展卡尔曼滤波原理
拓展卡尔曼滤波原理一、引言卡尔曼滤波是一种用于估计系统状态的优化滤波方法，广泛应用于控制系统、导航系统、信号处理等领域。
然而，在某些实际应用中，标准的卡尔曼滤波可能无法满足需求。
因此，拓展卡尔曼滤波（Extended Kalman Filter，EKF）应运而生。
本文将介绍拓展卡尔曼滤波的原理和应用。
二、传统卡尔曼滤波传统的卡尔曼滤波适用于线性系统，其核心思想是通过状态转移方程和观测方程来更新系统状态的估计值。
然而，实际系统往往是非线性的，因此传统卡尔曼滤波无法直接应用。
三、拓展卡尔曼滤波原理拓展卡尔曼滤波通过利用泰勒级数展开来近似非线性函数，从而使非线性系统也可以使用卡尔曼滤波进行状态估计。
其基本思想是在线性化的系统模型上运用卡尔曼滤波，通过线性化的方式来逼近非线性系统的真实行为。
具体而言，拓展卡尔曼滤波通过在非线性系统的状态转移方程和观测方程中引入雅可比矩阵（Jacobian matrix）来近似非线性函数。
雅可比矩阵是非线性函数在某一点的一阶偏导数矩阵，用于描述非线性函数的局部线性化。
在拓展卡尔曼滤波中，状态转移方程和观测方程的线性化可以分为两个步骤：预测步骤和更新步骤。
1. 预测步骤：预测步骤通过状态转移方程进行状态预测，并利用预测误差协方差矩阵来估计预测误差的不确定性。
在拓展卡尔曼滤波中，状态转移方程被线性化为一阶泰勒级数展开形式。
通过对非线性函数进行一阶泰勒级数展开，可以得到一个线性状态转移方程。
2. 更新步骤：更新步骤通过观测方程来更新状态估计值和预测误差协方差矩阵。
观测方程同样需要进行线性化，通过对非线性函数进行一阶泰勒级数展开来得到一个线性观测方程。
利用预测误差协方差矩阵和观测噪声协方差矩阵，可以计算卡尔曼增益，进而更新状态估计值和预测误差协方差矩阵。
四、拓展卡尔曼滤波应用拓展卡尔曼滤波在很多领域都有广泛的应用。
其中，最典型的应用之一是在导航系统中的位置和姿态估计。
由于导航系统中的传感器往往具有非线性特性，传统的卡尔曼滤波无法准确估计位置和姿态。
卡尔曼滤波理论的发展及现状
卡尔曼滤波理论的发展及现状作者：王娜赵衍年曹智明来源：《理论与创新》2018年第04期摘要：随着计算机技术的发展，卡尔曼滤波信息融合算法作为目标跟踪中常用的信息处理方法，能够对目标实时并准确地跟踪。
在线性系统中，卡尔曼滤波就是最优滤波器，其处理高斯模型的系统也非常有效，目前卡尔曼滤波理论已经广泛应用在国防、军事、跟踪、制导等许多高科技领域。
关键词：卡尔曼滤波；无迹卡尔曼滤波器；UT变换；UKF滤波卡尔曼滤波理论20世纪60年代，著名科学家卡尔曼（R. E. Kalman）首次提出了解决离散系统现行滤波问题的递归算法，后人将这种方法命名为卡尔曼滤波，以纪念卡尔曼在滤波理论方面做出的突出贡献。
作为一种首选的最优估计理论，卡尔曼滤波已经逐渐成为估计与预测状态空间模型的强有力工具之一，在组合导航领域及惯性导航领域得到了越来越广泛的运用。
在某种特定情况下，系统的线性数学模型的确能够体现系统的特点和性能。
然而，对于任何实际系统，都不同程度存在非线性，当系统的非线性并不强时，这些系统可近似当成线性系统处理，而对于绝大多数实际系统，线性数学模型并不足以描述其特性，系统中的非线性因素并不能被忽略。
为了能够以较高的精度和较快的计算速度处理非线性高斯系统的滤波问题，Julier等人根据确定性采样的基本思想，基于Unscented变换（UT变换）提出了无迹卡尔曼滤波。
UT变换按照加权统计线性回归来计算随机变量的后验概率分布，由系统变量的先验统计特性，采用特定的采样策略产生一系列采样点，即Sigma点，对生成的Sigma点通过系统状态方程传播后得到系统状态采样点，利用生成的Sigma采样点对对非线性分布进行线性化近似，然后基于生成的系统状态采样点计算状态的后验概率统计。
UT变换将统看成“黑箱”模型，并不依赖具体的系统形式，而且无需计算Jacobian矩阵。
卡尔曼滤波理论发展及现状滤波就是将所需要的有用信号从从混合在一起的各种信号中提取出来。
UKF
k / k 1
Z k ≈ H k X k + Vk
由线性化模型就可以根据Kalman滤波方程进行递推 计算：
X k / k 1 = f ( X k 1 )
X k = X k / k 1 + K k [ Z k h( X k / k 1 )]
T T K k = Pk / k 1 H k [ H k Pk / k 1 H k + Rk ]1
UKF的基本原理
设连续线性系统带随机干扰下的微分方程式为：
& X ( t ) = AX ( t ) + GW ( t )
式中：A、G是时变量；W是白噪声过程且方差 为Q 离散化的状态方程为 X Q = ΦX + W
k +1 k k
式中：Φ = e
AT
= I + AT +
AT +L 2!
2
2
Wk的噪声序列方差为Qk ( AGQG + GQG Q = GQG T +
k 1
k / k 1
EKF递推算法
EKF围绕状态估计值 X 将非线性函数f()展成泰勒级数并取其
k 1
一阶近似，围绕状态预测值 X 将非线性函数h()展成泰勒级数并取
其一阶近似，从而得到非线性系统的近似线性化模型： X k ≈ Φ k / k 1 X k 1 + Γ k 1Wk 1
Φ 和 其中，k / k 1 H k 分别称为向量函数f()和h()的Jacobian矩阵，
T T k
T
AT ) T 2
2!
+L
Qk中取第一项，则有结论：状态的方差乘以T，这是 离散后的结论。
非线性离散系统由前面的推导可假设随机非线性离散系统 为： X k = f ( X k 1 ) + Γ k 1Wk 1
EKF、UKF和CKF的滤波性能对比研究
EKF、UKF和CKF的滤波性能对比研究常宇健;赵辰【摘要】普通卡尔曼滤波(KF)可以在线性系统中对目标状态做出最优估计,得到好的滤波效果.然而实际系统总是非线性的,针对非线性系统,常用的解决办法是对非线性系统进行近似线性化处理,从而将非线性问题转变成线性问题.文中分析了扩展卡尔曼(EKF)、无迹卡尔曼(UKF)和容积卡尔曼(CKF)的基本原理和各自的特点,然后将EKF、UKF和CKF进行滤波对比和分析,最后通过仿真试验证明:与EKF相比,UKF、CKF不仅保证了系统的稳定性,同时提高了估计精度.但CKF的估计均方误差值相比UKF更小,表现出了更高的精度.【期刊名称】《石家庄铁道大学学报（自然科学版）》【年(卷),期】2019(032)002【总页数】7页(P104-110)【关键词】扩展卡尔曼;无迹卡尔曼;容积卡尔曼;非线性滤波;估计精度【作者】常宇健;赵辰【作者单位】石家庄铁道大学电气与电子工程学院,河北石家庄050043;河北省交通安全与控制重点实验室,河北石家庄050043;石家庄铁道大学电气与电子工程学院,河北石家庄050043【正文语种】中文【中图分类】TP391.90 引言滤波是指将有用信号中的噪声进行滤除的一种方法。
生活中的系统基本都是非线性的，针对非线性系统的滤波方法主要有EKF、UKF和CKF。
EKF是将非线性函数展开成泰勒级数并略去高阶项，一般适用于弱非线性系统。
EKF相对UKF、CKF较为简单且容易实现，但在强非线性情况下滤波性能差，且计算量相比UKF和CKF大，有时会导致滤波发散[1]。
UKF则用UT变换对均值和协方差的非线性传递进行处理，因此其精度高于EKF。
UKF广泛应用于目标跟踪、汽车行驶状态估计、导航系统和飞行器飞行姿态估计等领域。
UKF虽然克服了EKF易发散的缺点，但在高维系统中UKF需要合理地调节参数才能得到好的滤波效果[2]，应用比较困难。
最近，基于Cubature变换的CKF滤波算法由Arasaratnam et al提出 [3-4]，该算法在状态估计、机动目标定位等领域得到广泛的应用。
EKF_UKF_PF目标跟踪性能的比较_万莉
=
1 N
δ(xk
j =1
-
xkj )
(9)
粒子滤波的两个关键问题是参考分布 q(xk |
xk-1 , zk) 的选择和重采样 。
粒子滤 波对状态估计的好坏 , 在很大程度上
取决于所选的参考分布与状态后验概率分布的接
(6)
UKF 是用确定的采样来近似状态的后验 PDF ,
可以有效解决由系统非线性的加剧而引起的滤波发
散问题 , 但 UKF 仍是用高斯分布来逼近系统状态的 后验概率密度 , 所以在系统状态的后验概率密度是 非高斯的情况下[ 3] ,滤波结果将有极大的误差 。
2. 2. 2 粒子滤波 PF 粒子滤波利用一系列带权值的空间随机采样 的粒子来逼近后验概率密度函数 , 是一种基于 M onte Carlo 的贝叶斯估计方法 , 因此它就独立于
状态方程 : Xk = f k(X k-1 , vk-1 ) =F * Xk- 1 +vk-1 (1)
量测方程 :
Zk =hk(Xk , ωk) =
x2k +y2k t an-1 (y k / xk )
+ωk
(2)
式中 , vk是状态噪声 , ωk是过程噪声 , 两者相互独立 。
2. 1 函数近似法 ———EKF
0
Wi =(nχk+k) i = 0
χ =χ+(
i
(n χ+k)Pχ)i
Wi
=
k 2(nχ+k)
i
=1,
…,
nχ
χ
i+n χ
= χ-
(
(nχ+k)Pχ)i
W i +n χ
=
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
二、扩展Kalman滤波(EKF)算法
vy = vy + (ky*vy^2-g+day*randn(1))*Ts; X(k,:) = [x, vx, y, vy]; end figure(1), hold off, plot(X(:,1),X(:,3),'-b'), grid on % figure(2), plot(X(:,2:2:4)) % 构造量测量 mrad = 0.001; dr = 10; dafa = 10*mrad; % 量测噪声 for k=1:len r = sqrt(X(k,1)^2+X(k,3)^2) + dr*randn(1,1); a = atan(X(k,1)/X(k,3)) + dafa*randn(1,1); Z(k,:) = [r, a]; end
成线性问题。 对于非线性问题线性化常用的两大途径：
（1） 将非线性环节线性化，对高阶项采用忽略或逼近措施；（EKF）
（2）用采样方法近似非线性分布. ( UKF)
二、扩展Kalman滤波(EKF)算法
EKF算法是一种近似方法，它将非线性模型在状态 估计值附近作泰勒级数展开，并在一阶截断，用得 到的一阶近似项作为原状态方程和测量方程近似表 达形式，从而实现线性化同时假定线性化后的状态 依然服从高斯分布，然后对线性化后的系统采用标 准卡尔曼滤波获得状态估计。采用局部线性化技术， 能得到问题局部最优解，但它能否收敛于全局最优 解，取决于函数的非线性强度以及展开点的选择。
二、扩展Kalman滤波(EKF)算法
figure(1), plot(X_est(:,1),X_est(:,3), '+r') xlabel('X'); ylabel('Y'); title('ekf simulation'); legend('real', 'measurement', 'ekf estimated'); %%%%%%%%%%%%%%%%%%%%子程序%%%%%%%%%%%%%%%%%%% function F = JacobianF(X, kx, ky, g) % 系统状态雅可比函数 = X(2); vy = X(4); F = zeros(4,4); F(1, F(2,2) = -2*kx*vx; F(3,4) = 1; F(4,4) = 2*ky*vy; 2) = 1;
h( X k ) H (K ) X
X X ( k k 1)
.......... .(4)
二、扩展Kalman滤波(EKF)算法
将线性化后的状态转移矩阵和观测矩阵代入到标 准卡尔曼滤波框架中，即得到扩展卡尔曼滤波。 因为EKF忽略了非线性函数泰勒展开的高阶项， 仅仅用了一阶项，是非线性函数在局部线性化的结果， 这就给估计带来了很大误差，所以只有当系统的状态 方程和观测方程都接近线性且连续时，EKF的滤波结 果才有可能接近真实值。EKF滤波结果的好坏还与状 态噪声和观测噪声的统计特性有关，在EKF的递推滤 波过程中，状态噪声和观测噪声的协方差矩阵保持不 变，如果这两个噪声协方差矩阵估计的不够准确，那 就容易产生误差累计，导致滤波器发散。EKF的另外 一个缺点是初始状态不太好确定，如果假设的初始状 态和初始协方差误差较大，也容易导致滤波器发散。
二、扩展Kalman滤波(EKF)算法
Matlab程序： function test_ekf kx = .01; ky = .05; % 阻尼系数 g = 9.8; % 重力 t = 10; % 仿真时间 Ts = 0.1; % 采样周期 len = fix(t/Ts); % 仿真步数 % 真实轨迹模拟 dax = 1.5; day = 1.5; % 系统噪声 X = zeros(len,4); X(1,:) = [0, 50, 500, 0]; % 状态模拟的初值 for k=2:len x = X(k-1,1); vx = X(k-1,2); y = X(k-1,3); vy = X(k-1,4); x = x + vx*Ts; vx = vx + (-kx*vx^2+dax*randn(1,1))*Ts; y = y + vy*Ts;
二、扩展Kalman滤波(EKF)算法
figure(1), hold on, plot(Z(:,1).*sin(Z(:,2)), Z(:,1).*cos(Z(:,2)),'*') % ekf 滤波 Qk = diag([0; dax; 0; day])^2; Rk = diag([dr; dafa])^2; Xk = zeros(4,1); Pk = 100*eye(4); X_est = X; for k=1:len Ft = JacobianF(X(k,:), kx, ky, g); Hk = JacobianH(X(k,:)); fX = fff(X(k,:), kx, ky, g, Ts); hfX = hhh(fX, Ts); [Xk, Pk, Kk] = ekf(eye(4)+Ft*Ts, Qk, fX, Pk, Hk, Rk, Z(k,:)'-hfX); X_est(k,:) = Xk'; end
EKF与UKF
一、背景
普通卡尔曼滤波是在线性高斯情况下利用最小均方误差准则获得
目标的动态估计，适应于过程和测量都属于线性系统， 且误差符
合高斯分布的系统。 但是实际上很多系统都存在一定的非线性， 表现在过程方程 （状态方程）是非线性的，或者观测与状态之间 的关系（测量方程）是非线性的。这种情况下就不能使用一般的卡 尔曼滤波了。解决的方法是将非线性关系进行线性近似，将其转化
二、扩展Kalman滤波(EKF)算法
假定定位跟踪问题的非线性状态方程和测量方程如 下: X k 1 f ( X k ) Wk .......... .....( 1)
Yk h( X k ) Vk
.......... .......... (2)
在最近一次状态估计的时刻，对以上两式进 行线性化处理，首先构造如下2个矩阵: f ( X k ) F (k 1 k ) .......( 3) X X (k k ) X