拓展卡尔曼滤波
ekf算法工作原理
ekf算法工作原理1.前言扩展卡尔曼滤波(Extended Kalman Filter, EKF)是一种常见的状态估计算法，常用于机器人感知与定位、飞行器控制等领域。
本文将介绍EKF算法的基本原理，以及其在机器人感知与定位中的应用。
2. EKF算法概述2.1 EKF算法的基本原理EKF算法是卡尔曼滤波( Kalman Filter )的一种扩展形式，其基本思路是通过利用系统的非线性动态方程进行线性近似，然后运用标准的卡尔曼滤波理论对系统进行状态估计。
相对于卡尔曼滤波，EKF算法的主要区别在于：卡尔曼滤波要求系统的性质必须是线性、高斯、马尔可夫的，而EKF算法可以应用于一定范围内的非线性系统，但是仍然要求系统噪声必须是高斯噪声。
在EKF算法中，需要定义系统的状态量、测量量、动态方程和测量方程。
其中，状态量和测量量为向量形式，动态方程和测量方程为非线性的函数形式。
在进行状态估计时，EKF算法将动态方程和测量方程在当前状态附近进行泰勒展开，最终得到一个线性模型。
通过对该模型进行预测和校正，可以得到对系统状态的估计值。
2.2 EKF算法的基本流程EKF算法的基本流程可以分为以下几个步骤：1）初始化：设定系统的初始化状态及其方差。
2）预测：根据当前状态的动态方程对下一时刻的状态进行预测。
3）预测方差：计算预测的状态方差。
4）校正：利用当前时刻的测量方程对预测结果进行校正。
5）校正方差：根据校正时产生的残差计算当前状态估计的方差。
6）返回第二步。
3. EKF算法的应用3.1 机器人感知与定位EKF算法在机器人感知和定位中的应用日益广泛。
机器人的感知和定位问题通常是非线性的，包括地图构建、自我定位、目标检测等问题。
EKF算法可以用于机器人的状态估计，例如机器人的位置、姿态、速度等。
同时，扩展卡尔曼滤波还可以处理数据关联的问题，例如由多个传感器提供的数据，并且可以消除错误信息，提高估计的精度。
3.2 飞行器控制EKF算法在飞行器控制中的应用也非常广泛。
扩展卡尔曼滤波推导
扩展卡尔曼滤波推导卡尔曼滤波是现代控制理论与技术的重要组成部分，主要用于估计目标系统的状态并输出最优的状态估计值。
但随着系统复杂度的提高，传统的卡尔曼滤波往往无法满足要求，于是扩展卡尔曼滤波（EKF）应运而生。
扩展卡尔曼滤波是在卡尔曼滤波基础上发展而来的一种滤波方法，其主要应用于非线性、高斯噪声系统的状态估计。
在EKF中，通过将非线性函数在当前状态处进行泰勒展开，然后利用卡尔曼滤波技术对线性化后的系统进行状态估计。
在扩展卡尔曼滤波的推导中，需要对状态方程和观测方程进行非线性函数的泰勒展开，具体而言，如果状态方程为：$$x_{k+1}=f(x_k,u_k)+w_k$$其中，$f(x_k,u_k)$为状态转移函数，$w_k$为高斯噪声，那么对其进行泰勒展开可得：$$x_{k+1}\approx f(\hat{x_k},u_k)+(F_k)(x_k-\hat{x_k})+w_k$$其中，$F_k$为状态转移函数在$\hat{x_k}$处的雅可比矩阵，$x_k-\hat{x_k}$为状态的偏差。
同样地，如果观测方程为：$$z_{k}=h(x_k)+v_k$$其中，$h(x_k)$为观测函数，$v_k$为高斯噪声，那么对其进行泰勒展开可得：$$z_{k}\approx h(\hat{x_k})+(H_k)(x_k-\hat{x_k})+v_k$$其中，$H_k$为观测函数在$\hat{x_k}$处的雅可比矩阵。
接下来，根据卡尔曼滤波的基本公式，可以得到扩展卡尔曼滤波的递推公式：预测步：$$\hat{x}_{k+1}^{-}=f(\hat{x_k},u_k)$$$$P_{k+1}^{-}=F_kP_kF_k^T+Q_k$$其中，$Q_k$为过程噪声的方差协方差矩阵。
更新步：$$K_k=P_k^{-}H_k^T(H_kP_k^{-}H_k^T+R_k)^{-1}$$$$\hat{x_k}=\hat{x_k}^{-}+K_k(z_k-h(\hat{x_k}^{-}))$$$$P_k=(I-K_kH_k)P_k^{-}$$其中，$R_k$为测量噪声的方差协方差矩阵。
扩展卡尔曼滤波 调参
扩展卡尔曼滤波调参1. 什么是卡尔曼滤波？卡尔曼滤波（Kalman Filter）是一种用于估计系统状态的递归滤波器。
它能够通过融合来自传感器的测量数据和系统模型的预测值，提供对系统状态的最优估计。
卡尔曼滤波器的核心思想是通过不断迭代的方式，根据当前的观测值和先验估计值，计算出最优的后验估计值。
它的优点在于对于线性系统，能够得到最优解，并且具有较低的计算复杂度。
2. 扩展卡尔曼滤波（Extended Kalman Filter，EKF）扩展卡尔曼滤波是卡尔曼滤波的一种扩展，用于非线性系统的状态估计。
与传统的卡尔曼滤波相比，扩展卡尔曼滤波能够通过线性化非线性系统模型，将其转化为线性系统模型，从而实现状态的估计。
在扩展卡尔曼滤波中，通过使用泰勒级数展开，将非线性函数线性化为一阶导数的形式。
然后，使用线性卡尔曼滤波的方法进行状态估计。
这样一来，扩展卡尔曼滤波能够处理一些非线性系统，并提供对系统状态的最优估计。
3. 扩展卡尔曼滤波调参在使用扩展卡尔曼滤波进行状态估计时，需要对滤波器进行一些参数的调整，以获得更好的估计结果。
下面介绍一些常用的调参方法。
3.1 系统模型在使用扩展卡尔曼滤波进行状态估计时，首先需要定义系统的状态方程和观测方程。
系统的状态方程描述了系统状态的演化规律，而观测方程描述了观测值与系统状态之间的关系。
在调参时，需要根据实际情况对系统模型进行调整。
对于非线性系统，可以通过改变状态方程和观测方程的形式，使其更好地与实际系统相匹配。
3.2 过程噪声和观测噪声在卡尔曼滤波中，过程噪声和观测噪声是用来描述系统模型和观测模型中的不确定性的参数。
过程噪声表示系统状态的演化过程中的不确定性，观测噪声表示观测值的不确定性。
在调参时，需要根据实际情况对过程噪声和观测噪声进行调整。
过程噪声和观测噪声的大小与系统的动态特性和传感器的性能有关。
通过调整这两个参数，可以使滤波器更好地适应实际情况。
3.3 初始状态和协方差在卡尔曼滤波中，初始状态和协方差用来表示对系统状态的初始估计。
扩展卡尔曼滤波雅可比矩阵
扩展卡尔曼滤波雅可比矩阵扩展卡尔曼滤波（Extended Kalman Filter, EKF）是一种用于非线性系统状态估计的优化算法。
而雅可比矩阵则是EKF中的重要工具，用于线性化系统中的非线性函数。
在实际应用中，许多系统的动态特性往往具有非线性的特点，这意味着传统的卡尔曼滤波算法无法直接应用于这些系统。
为了解决这一问题，EKF被提出。
EKF通过对系统状态和观测方程进行线性化，将非线性系统转化为线性系统，从而利用卡尔曼滤波算法进行状态估计。
而在EKF中，雅可比矩阵扮演着至关重要的角色。
雅可比矩阵本质上是非线性函数在某一点的线性化矩阵。
它通过对非线性函数进行一阶泰勒展开，近似地计算了函数在该点的变化率。
因此，雅可比矩阵的计算是EKF中一个关键的环节。
在实际应用中，计算雅可比矩阵有很多种方法，最常用的是数值差分法和解析法。
数值差分法通过取小的增量，计算函数在该点的微分，然后将微分值作为该点的近似导数。
这种方法简单直观，但由于计算误差和数值不稳定性的存在，可能会影响到滤波器的性能。
而解析法则是通过求取函数的一阶偏导数来计算雅可比矩阵。
这种方法通常要求对非线性函数进行符号推导，然后计算导数。
虽然解析法的计算精度更高，但由于非线性函数的复杂性，求解过程可能会比较困难和繁琐。
因此，在实际应用中，选择合适的计算雅可比矩阵的方法是非常重要的。
数值差分法适用于简单的非线性函数，而解析法则适用于复杂的非线性函数。
根据系统的特性和性能需求，我们可以选择最适合的方法。
总之，扩展卡尔曼滤波雅可比矩阵在非线性系统状态估计中具有重要的意义。
通过对系统进行线性化，EKF能够有效地处理非线性系统，并提供准确的状态估计结果。
雅可比矩阵的计算方法因应用而异，选择适合的方法是保证滤波器性能的关键。
在未来的研究和应用中，我们可以进一步探索改进雅可比矩阵计算方法的技术，以提高EKF的性能和适用范围。
扩展卡尔曼滤波算法
扩展卡尔曼滤波算法1 卡尔曼滤波算法卡尔曼滤波（Kalman Filter，KF）是指根据系统过程的当前测量值来估计未来某时刻的状态参量值的算法。
它可以帮助我们进行最优估计和状态跟踪辨识，在实际应用中一般用于非线性系统的实时状态值的估计及系统的控制、导航定位和信号处理等密切相关的任务。
卡尔曼滤波算法根据观测结果及自身的建模，以多次观测水深数据为重点，将观测结果和系统估计值进行更新和修正，从而获得一种逐次改进的过程模型，从而得出更准确的系统状态估计值。
2 扩展卡尔曼滤波算法基于卡尔曼滤波算法的扩展技术，是普遍存在的技术，它集合了计算机、数据处理和系统建模的原理，可以更先进的估计数据和追踪目标，最常用的方法被称为扩展卡尔曼滤波（EKF）。
该算法包括线性和非线性估计，可以扩展表达能力，从而结合卡尔曼滤波算法带来的传感精度和稳定性，使物体行进轨迹推测、跟踪更准确。
3 应用扩展卡尔曼滤波算法的应用领域包括空气制动原理应用、机器视觉方位估计、太阳能机器人位置跟踪、磁测量器定位、自动攻击模块偏转角识别等，以及虚拟地铁位置估计和导航，用于智能领域的研究。
在机器人导航研究中，扩展卡尔曼滤波算法可以在环境变化较多或污染较大的条件下，快速实现机器人位置估计和路径规划，满足快速智能系统设计的需求。
4 小结扩展卡尔曼滤波算法是利用卡尔曼滤波算法所提供的精度、稳定性和可扩展性，发展出来的一种滤波技术。
它可以合理地估计和预测某系统的状态，并及时追踪物体行走的轨迹，有效的计算系统的位置，有利于智能系统、机器人导航系统以及虚拟实验系统的设计，从而使系统的优化以及最优化更贴近实际应用。
扩展卡尔曼粒子滤波优化
扩展卡尔曼粒子滤波优化扩展卡尔曼粒子滤波优化扩展卡尔曼粒子滤波（Extended Kalman Particle Filter）是一种常用的非线性滤波器，用于在测量数据包含噪声的情况下对系统状态进行估计。
在本文中，我们将逐步介绍扩展卡尔曼粒子滤波的优化方法。
1. 了解卡尔曼滤波：在深入研究扩展卡尔曼粒子滤波之前，首先需要了解卡尔曼滤波的基本原理。
卡尔曼滤波是一种递归滤波器，通过使用系统模型和测量模型来估计系统状态，其中系统模型和测量模型通常是线性的。
然而，实际应用中，我们经常遇到非线性问题，这就需要使用扩展卡尔曼滤波。
2. 扩展卡尔曼滤波：扩展卡尔曼滤波通过在线性化非线性系统模型和测量模型来处理非线性问题。
具体而言，它使用泰勒级数展开来近似非线性函数，并通过计算雅可比矩阵来线性化系统模型和测量模型。
然后，通过应用卡尔曼滤波来处理线性化的系统和测量模型。
3. 粒子滤波：粒子滤波是一种基于蒙特卡洛方法的滤波器，用于处理非线性和非高斯的系统。
它通过使用一组粒子来表示系统的状态分布，并通过对每个粒子进行重采样和更新来更新状态分布。
这使得粒子滤波能够更好地适应非线性和非高斯分布的系统。
4. 扩展卡尔曼粒子滤波：扩展卡尔曼粒子滤波是将扩展卡尔曼滤波和粒子滤波相结合的一种方法。
它使用粒子滤波来近似状态分布，并通过在线性化非线性系统模型和测量模型来处理非线性问题。
具体而言，它使用粒子滤波来生成一组粒子，然后通过计算每个粒子的权重来更新状态分布。
最后，通过对具有较高权重的粒子进行重采样来更新粒子集合。
5. 优化方法：为了进一步优化扩展卡尔曼粒子滤波，可以采取以下方法：- 调整粒子数目：增加粒子的数量可以提高滤波器的精度，但也会增加计算复杂度。
因此，需要权衡精度和计算复杂度之间的关系，选择适当的粒子数目。
- 选择合适的重采样方法：重采样是粒子滤波的关键步骤，可以通过对低权重粒子进行更多采样来提高滤波器的性能。
常用的重采样方法包括系统性重采样、残差重采样和分层重采样等。
基于扩展卡尔曼滤波的目标跟踪定位算法及matlab程序实现
基于扩展卡尔曼滤波的目标跟踪定位算法及matlab程序实现扩展卡尔曼滤波（Extended Kalman Filter，EKF）是一种用于非线性系统状态估计的算法。
在目标跟踪定位中，它可以用于估计目标的运动轨迹。
下面是一个简单的基于扩展卡尔曼滤波的目标跟踪定位算法的描述，以及一个简化的MATLAB程序实现。
算法描述1. 初始化：设置初始状态估计值（例如位置和速度）以及初始的估计误差协方差矩阵。
2. 预测：根据上一时刻的状态估计值和模型预测下一时刻的状态。
3. 更新：结合观测数据和预测值，使用扩展卡尔曼滤波算法更新状态估计值和估计误差协方差矩阵。
4. 迭代：重复步骤2和3，直到达到终止条件。
MATLAB程序实现这是一个简化的示例，仅用于说明扩展卡尔曼滤波在目标跟踪定位中的应用。
实际应用中，您需要根据具体问题和数据调整模型和参数。
```matlab% 参数设置dt = ; % 时间间隔Q = ; % 过程噪声协方差R = 1; % 观测噪声协方差x_est = [0; 0]; % 初始位置估计P_est = eye(2); % 初始估计误差协方差矩阵% 模拟数据：观测位置和真实轨迹N = 100; % 模拟数据点数x_true = [0; 0]; % 真实轨迹初始位置for k = 1:N% 真实轨迹模型（这里使用简化的匀速模型）x_true(1) = x_true(1) + x_true(2)dt;x_true(2) = x_true(2);% 观测模型（这里假设有噪声）z = x_true + sqrt(R)randn; % 观测位置% 扩展卡尔曼滤波更新步骤[x_est, P_est] = ekf_update(x_est, P_est, z, dt, Q, R);end% 扩展卡尔曼滤波更新函数（这里简化为2D一维情况）function [x_est, P_est] = ekf_update(x_est, P_est, z, dt, Q, R)% 预测步骤：无观测时使用上一时刻的状态和模型预测下一时刻状态F = [1 dt; 0 1]; % 状态转移矩阵（这里使用简化的匀速模型）x_pred = Fx_est + [0; 0]; % 预测位置P_pred = FP_estF' + Q; % 预测误差协方差矩阵% 更新步骤：结合观测数据和预测值进行状态更新和误差协方差矩阵更新K = P_predinv(HP_pred + R); % 卡尔曼增益矩阵x_est = x_pred + K(z - Hx_pred); % 更新位置估计值P_est = (eye(2) - KH)P_pred; % 更新误差协方差矩阵end```这个示例代码使用扩展卡尔曼滤波对一个简化的匀速运动模型进行估计。
ekf算法公式
ekf算法公式EKF（扩展卡尔曼滤波）算法公式如下：1.预测步骤：o状态预测：( \hat{x}{k|k-1} = f(\hat{x}{k-1|k-1}, u_k) )o协方差预测：( P_{k|k-1} = F_kP_{k-1|k-1}F_k^T + Q_k )其中，( \hat{x}{k|k-1} ) 是状态预测值，( \hat{x}{k-1|k-1} ) 是上一时刻的最优估计值，( u_k ) 是控制输入，( f ) 是非线性状态转移函数，( F_k ) 是( f ) 关于状态量的雅可比矩阵，( P_{k|k-1} ) 是协方差预测值，( P_{k-1|k-1} ) 是上一时刻的最优协方差，( Q_k ) 是过程噪声协方差。
2.更新步骤：o卡尔曼增益：( K_k = P_{k|k-1}H_k^T(H_kP_{k|k-1}H_k^T + R_k)^{-1} )o状态更新：( \hat{x}{k|k} = \hat{x}{k|k-1} + K_k(z_k -h(\hat{x}_{k|k-1})) )o协方差更新：( P_{k|k} = (I - K_kH_k)P_{k|k-1} )其中，( K_k ) 是卡尔曼增益，( H_k ) 是量测函数( h ) 关于状态量的雅可比矩阵，( z_k ) 是量测值，( h ) 是非线性量测函数，( R_k ) 是量测噪声协方差，( \hat{x}{k|k} ) 是状态更新值，( P{k|k} ) 是协方差更新值，( I ) 是单位矩阵。
需要注意的是，EKF算法中涉及到的非线性函数需要进行线性化处理，通常使用泰勒展开式进行近似。
在实际应用中，EKF算法还需要根据具体问题进行适当的调整和优化。
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
南京航空航天大学随机信号小论文题目扩展卡尔曼滤波学生姓名梅晟学号SX1504059学院电子信息工程学院专业通信与信息系统扩展卡尔曼滤波一、引言20世纪60年代，在航空航天工程突飞猛进而电子计算机又方兴未艾之时，卡尔曼发表了论文《A New Approach to Linear Filtering and Prediction Problems》（一种关于线性滤波与预测问题的新方法），这让卡尔曼滤波成为了时域内有效的滤波方法，从此各种基于卡尔曼滤波的方法横空出世，在目标跟踪、故障诊断、计量经济学、惯导系统等方面得到了长足的发展。
二、卡尔曼滤波器卡尔曼滤波是一种高效率的递归滤波器(自回归滤波器), 它能够从一系列的不完全及包含噪声的测量中，估计动态系统的状态。
卡尔曼滤波的一个典型实例是从一组有限的，包含噪声的，对物体位置的观察序列（可能有偏差）预测出物体的位置的坐标及速度。
卡尔曼在NASA埃姆斯研究中心访问时，发现他的方法对于解决阿波罗计划的轨道预测很有用，后来阿波罗飞船的导航电脑便使用了这种滤波器。
目前,卡尔曼滤波已经有很多不同的实现。
卡尔曼最初提出的形式现在一般称为简单卡尔曼滤波器。
除此以外，还有施密特扩展滤波器、信息滤波器以及很多Bierman, Thornton 开发的平方根滤波器的变种。
也许最常见的卡尔曼滤波器是锁相环，它在收音机、计算机和几乎任何视频或通讯设备中广泛存在。
三、扩展卡尔曼滤波器3.1 被估计的过程信号卡尔曼最初提出的滤波理论只适用于线性系统,Bucy，Sunahara等人提出并研究了扩展卡尔曼滤波(Extended Kalman Filter，简称EKF)，将卡尔曼滤波理论进一步应用到非线性领域。
EKF的基本思想是将非线性系统线性化，然后进行卡尔曼滤波，因此EKF是一种次优滤波。
同泰勒级数类似，面对非线性关系时，我们可以通过求过程和量测方程的偏导来线性化并计算当前估计。
假设过程具有状态向量x∈ℜn，其状态方程为非线性随机差分方程的形式。
x k=f x k−1,u k−1,w k−1(1.1)观测变量z∈ℜm为：z k=ℎ(x k,v k)(1.2)随机变量w k和v k代表过程激励噪声和观测噪声。
它们为相互独立，服从正态分布的白色噪声：p(w) ∼N(0, Q),p(v) ∼N(0, R).实际系统中，过程激励噪声协方差矩阵Q 和观测噪声协方差矩阵R 可能会随每次迭代计算而变化。
但在这我们假设它们是常数。
差分方程式(1.1)中的非线性函数f 将过去k −1 时刻状态与现在k 时刻状态联系起来。
量测方程(1.2)中的驱动函数u k和零均值过程噪声w k是它的参数。
非线性函数h 反映了状态变量x k和观测变量z k的关系。
实际中我们显然不知道每一时刻噪声w k和v k各自的值。
但是，我们可以将它们假设为零，从而估计状态向量和观测向量为：x k=f x k−1,u k−1,0(1.3)和z k=ℎ(x k,0)(1.4)其中，x k是过程相对前一时刻k 的后验估计。
有一点非常重要，那就是扩展卡尔曼滤波器的一个基本缺陷：离散随机变量的分布（或连续随机变量的密度）在经过非线性系统转化后不再是正态的了。
扩展卡尔曼滤波器其实就是一个通过线性化而达到渐进最优贝叶斯决策的特殊状态估计器。
3.2 滤波器的计算原型为了估计一个具有非线性差分和量测关系的过程，我们先给出式(1.3)和式(1.4)的一个新的线性化表示：x k≈x k+A x k−1−x k−1+Ww k−1(1.5)z k≈z k+H x k−x k+Vv k(1.6)其中，• x k和z k是状态向量和观测向量的真值，• x k和z k来自1.3式和1.4式，是状态向量和观测向量的近似值，• x k是k 时刻状态向量的后验估计，•随机变量w k和v k表示过程激励噪声和观测噪声。
• A 是f 对x 的偏导的雅可比矩阵：A[i,j]=ðf ijx k−1,u k−1,0• W 是f 对w 的偏导的雅可比矩阵：W[i,j]=ðf iðw jx k−1,u k−1,0• H 是h 对x 的偏导的雅可比矩阵：H[i,j]=ðℎiðx jx k,0• V 是h 对v 的偏导的雅可比矩阵：V[i,j]=ðℎijx k,0现在我们定义一个新的预测误差的表达式：e xk≡x k−x k(1.7)和观测变量的残余，e zk≡z k−z k(1.8)但实际中无法获得(1.7)式中的x k,它是状态向量的真值，也就是要估计的对象。
同样，(1.8)式中的z k也是无法获取的，它是用来估计x k的观测向量的真值。
由(1.7)式和(1.8)式我们可以写出误差过程的表达式：e xk≈A x k−1−x k−1+ϵk(1.9)e zk ≈He xk+ηk(1.10)ϵk和ηk代表具有零均值和协方差矩阵WQW T和VRV T的独立随机变量，Q 和R 分别为过程激励噪声协方差矩阵和观测噪声协方差矩阵。
在此我们利用(1.8)式中的观测残余真值e zk 去估计(1.9)式中的预测误差e xk，估计结果记为e k，结合(1.7)式可以获得初始非线性过程的后验状态估计：x k=x k+e k(1.11)(1.9)式和(1.10)式中的随机变量具有如下概率分布:p(e xk )~N(0,E[e xke xkT])p(ϵk)~N(0,WQ k W T)p(ηk)~N(0,VR k V T)令e k的估计值为零，由以上近似，可以写出估计e k的卡尔曼滤波器表达式：e k=K k e zk(1.12)将(1.8)式和(1.12)式代入(1.11)式，得到：x k=x k+K k e zk=x k+K k(z k−z k)(1.13) 3.3 拓展卡尔曼滤波总结扩展卡尔曼滤波器的一个重要特性是卡尔曼增益K k的表达式中的雅可比矩阵H k能够正确地传递或“加权”观测信息中的有用部分。
例如，如果通过h 观测变量z k和状态变量没有一一对应的关系，雅可比矩阵H k便通过改变卡尔曼增益从而使得残余z k−ℎ(x k−,0)中真正作用于状态变量的部分被加权。
当然，如果整个观测中观测变量z k和状态变量通过h 都没有一个一一对应的关系，那么滤波器很快就会发散。
这种情况下过程是不可观测的。
拓展卡尔曼滤波器基本运行流程图如下：四、Matlab仿真附程序如下:clc; close all; clear all;Xint_v = [1; 0; 0; 0; 0];wk = [1 0 0 0 0];vk = [1 0 0 0 0];for ii = 1:1:length(Xint_v)Ap(ii) = Xint_v(ii)*2;W(ii) = 0;H(ii) = -sin(Xint_v(ii));V(ii) = 0;Wk(ii) = 0;endUk = randn(1,200);Qu = cov(Uk);Vk = randn(1,200);Qv = cov(Vk);C = [1 0 0 0 0];n = 100;[YY XX] = EKLMNFTR1(Ap,Xint_v,Uk,Qu,Vk,Qv,C,n,Wk,W,V);for it = 1:1:length(XX)MSE(it) = YY(it) - XX(it);endtt = 1:1:length(XX);figure(1); subplot(211); plot(XX); title('ORIGINAL SIGNAL');subplot(212); plot(YY); title('ESTIMATED SIGNAL');figure(2); plot(tt,XX,tt,YY); title('Combined plot'); legend('original','estimated'); figure(3); plot(MSE.^2); title('Mean square error');%--------------------------------------------------------------------%--------------------------------------------------------------------function [YY,XX] = EKLMNFTR1(Ap,Xint_v,Uk,Qu,Vk,Qv,C,n,Wk,W,V);Ap(2,:) = 0;for ii = 1:1:length(Ap)-1Ap(ii+1,ii) = 1;endinx = 1;UUk = [Uk(inx); 0; 0; 0; 0];PPk = (Xint_v*Xint_v');VVk = [Vk(inx); 0; 0; 0; 0];Qv = V*V';for ii = 1:1:length(Xint_v)XKk(ii,1) = Xint_v(ii)^2; %FIRST STEP endPPk = Ap*PPk*Ap'; % SECOND STEPKk = PPk*C'*inv( (C*PPk*C') + (V*Qv*V') ); % THIRD STEPfor ii = 1:1:length(Xint_v)XUPK(ii,1) = XKk(ii)^2 + UUk(ii); % UPPER EQUATIONS.Zk(ii,1) = cos(XUPK(ii)) + VVk(ii); % UPPER EQUATIONS.endfor ii = 1:1:length(XKk)XBARk(ii,1)=XKk(ii)+ Kk(ii)*(Zk(ii) - (cos(XKk(ii)))) ; % FOURTH STEPendII = eye(5,5);Pk = ( II - Kk*C)*PPk; % FIFTH STEP %--------------------------------------------------------------------%--------------------------------------------------------------------for ii = 1:1:nUUk = [Uk(ii+1); 0; 0; 0; 0];PPk = XBARk*XBARk';VVk = [Vk(ii+1); 0; 0; 0; 0];XKk = exp(-XBARk); % FIRST STEP PPkM = Ap*PPk*Ap'; % SECOND STEPKk = PPkM*C'*inv( (C*PPkM*C') + (V*Qv*V') ); % THIRD STEPfornn = 1:1:length(XBARk)XUPK(nn) = exp(-XKk(nn)) + UUk(nn); % UPPER EQUATIONS. Zk(nn) = cos(XUPK(nn)) + VVk(nn); % UPPER EQUATIONS. endfor in = 1:1:length(XUPK)XNEW(in) = XBARk(in)+Kk(in)*(Zk(in) - cos(XBARk(in))); % FOURTH STEPendII = eye(5,5);Pk = (II - Kk*C)*PPkM; % FIFTH STEPXBARk = XNEW;OUTX(ii) = XBARk(1,1);OUTY(ii) = Zk(1,1);endYY = OUTY;XX = OUTX;。