利用扩展卡尔曼滤波算法进行目标状态估计
扩展卡尔曼滤波算法
扩展卡尔曼滤波算法1 卡尔曼滤波算法卡尔曼滤波（Kalman Filter，KF）是指根据系统过程的当前测量值来估计未来某时刻的状态参量值的算法。
它可以帮助我们进行最优估计和状态跟踪辨识，在实际应用中一般用于非线性系统的实时状态值的估计及系统的控制、导航定位和信号处理等密切相关的任务。
卡尔曼滤波算法根据观测结果及自身的建模，以多次观测水深数据为重点，将观测结果和系统估计值进行更新和修正，从而获得一种逐次改进的过程模型，从而得出更准确的系统状态估计值。
2 扩展卡尔曼滤波算法基于卡尔曼滤波算法的扩展技术，是普遍存在的技术，它集合了计算机、数据处理和系统建模的原理，可以更先进的估计数据和追踪目标，最常用的方法被称为扩展卡尔曼滤波（EKF）。
该算法包括线性和非线性估计，可以扩展表达能力，从而结合卡尔曼滤波算法带来的传感精度和稳定性，使物体行进轨迹推测、跟踪更准确。
3 应用扩展卡尔曼滤波算法的应用领域包括空气制动原理应用、机器视觉方位估计、太阳能机器人位置跟踪、磁测量器定位、自动攻击模块偏转角识别等，以及虚拟地铁位置估计和导航，用于智能领域的研究。
在机器人导航研究中，扩展卡尔曼滤波算法可以在环境变化较多或污染较大的条件下，快速实现机器人位置估计和路径规划，满足快速智能系统设计的需求。
4 小结扩展卡尔曼滤波算法是利用卡尔曼滤波算法所提供的精度、稳定性和可扩展性，发展出来的一种滤波技术。
它可以合理地估计和预测某系统的状态，并及时追踪物体行走的轨迹，有效的计算系统的位置，有利于智能系统、机器人导航系统以及虚拟实验系统的设计，从而使系统的优化以及最优化更贴近实际应用。
c语言实现扩展卡尔曼滤波
c语言实现扩展卡尔曼滤波
扩展卡尔曼滤波是一种常用于传感器融合的滤波算法，可以有效地估计目标的状态并减小测量误差。
在本文中，我将以人类的视角来描述扩展卡尔曼滤波的原理和应用。
让我简要介绍一下卡尔曼滤波的基本原理。
卡尔曼滤波是一种递归的滤波算法，通过预测和更新两个步骤来估计系统的状态。
在预测步骤中，根据系统的动态模型和先验信息，预测下一个状态的均值和方差。
然后，在更新步骤中，利用测量值对预测结果进行修正，得到最优的状态估计。
扩展卡尔曼滤波是卡尔曼滤波的一种扩展，可以处理非线性系统。
与传统的卡尔曼滤波不同，扩展卡尔曼滤波使用线性化的动态模型和测量模型来进行状态预测和更新。
具体而言，扩展卡尔曼滤波通过在每次迭代中对非线性函数进行线性化，将非线性系统转化为线性系统进行处理。
在实际应用中，扩展卡尔曼滤波广泛用于目标跟踪、导航和定位等领域。
例如，在自动驾驶汽车中，扩展卡尔曼滤波可以利用车载传感器（如雷达和摄像头）的测量数据，对车辆的位置和速度进行估计，从而实现精确的自动驾驶控制。
扩展卡尔曼滤波还可以应用于机器人定位和导航。
通过结合惯性测量单元（IMU）和全向视觉传感器的测量数据，扩展卡尔曼滤波可以
实现机器人在未知环境中的定位和导航。
这对于无人机、无人车等智能机器人的自主导航非常重要。
扩展卡尔曼滤波是一种强大的滤波算法，能够在非线性系统中有效地估计目标的状态。
它在传感器融合、目标跟踪和机器人导航等应用中发挥着重要的作用。
通过合理地选择动态模型和测量模型，并利用适当的线性化方法，我们可以利用扩展卡尔曼滤波来解决各种实际问题，并取得良好的估计效果。
deepsort 拓展卡尔曼滤波 -回复
deepsort 拓展卡尔曼滤波-回复deepsort是一种多对象跟踪算法，通过与卡尔曼滤波算法结合，为目标的跟踪提供更准确、更稳定的工具。
本文将详细介绍deepsort拓展卡尔曼滤波的实现方法和原理，从而帮助读者更好地理解和应用这一算法。
一、深度学习与目标跟踪目标跟踪是计算机视觉领域的一个重要任务，其目标是在视频序列中自动检测和跟踪特定目标的位置和运动。
在过去的几年里，深度学习已经在目标检测和分类等任务上取得了巨大成功，然而，在目标跟踪任务中，由于目标的外观变化、遮挡、运动模式的多样性等问题，深度学习方法往往难以取得理想的效果。
因此，结合深度学习和传统的目标跟踪方法是一种有效的解决方案。
二、deepsort算法简介deepsort算法是由NVIDIA提出的一种多对象跟踪算法。
它基于两个核心组件：一个是通过深度学习网络（如YOLO、Faster R-CNN等）进行目标检测和特征提取，另一个是通过卡尔曼滤波算法进行目标跟踪和状态估计。
在deepsort算法中，首先使用深度学习网络对视频序列进行目标检测，并提取每个目标的特征。
这些特征包括目标的外观特征、位置特征等，用于描述目标的状态。
然后，利用卡尔曼滤波算法对每个目标的状态进行动态建模和预测。
卡尔曼滤波算法基于贝叶斯推理原理，通过融合目标的测量信息和动态模型，对目标的状态进行估计和预测。
最后，通过特定的关联算法来匹配当前帧中的检测结果和上一帧中已经跟踪的目标，从而实现连续的多对象跟踪。
三、拓展卡尔曼滤波卡尔曼滤波是一种用于状态估计的优秀算法，但是由于其在实际应用中对系统动态模型的线性和高斯性假设，所以在面对非线性和非高斯的系统时，效果可能不理想。
为了解决这个问题，可以通过拓展卡尔曼滤波（EKF）来扩展卡尔曼滤波算法的适用范围。
拓展卡尔曼滤波主要针对非线性系统，通过在状态更新和测量更新步骤中引入线性化来近似非线性系统的动态模型和测量模型。
具体来说，在状态预测中，通过计算状态转移矩阵的一阶导数来线性化非线性函数；在测量更新中，通过计算观测矩阵的一阶导数来线性化非线性函数。
基于扩展卡尔曼滤波的目标跟踪定位算法及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```这个示例代码使用扩展卡尔曼滤波对一个简化的匀速运动模型进行估计。
状态估计的常用算法
状态估计的常用算法状态估计的常用算法状态估计是现代控制理论中重要的一环，其主要作用是通过测量数据对被控系统当前状态进行估计，便于进行后续控制处理。
实际上，在现代自动控制系统中，状态估计算法的应用范围非常广泛，包括物流自动化、车辆防控、机器人控制、航空航天系统等许多领域。
本文将针对状态估计的常用算法进行详细的介绍。
1.卡尔曼滤波卡尔曼滤波是状态估计的基本算法之一，其主要思想是基于时间序列的分析和预测。
卡尔曼滤波算法主要分为预测和更新两个过程，其中预测过程通过系统模型对下一个时间段的状态进行预测，而更新过程则通过测量量和预测量之间的差异进行状态估计的更新。
常见的卡尔曼滤波包括线性卡尔曼滤波、扩展卡尔曼滤波、粒子滤波等。
2.无迹卡尔曼滤波无迹卡尔曼滤波是卡尔曼滤波的一种改进算法，主要在卡尔曼滤波的过程中对协方差矩阵进行变换，避免出现协方差矩阵为负等问题。
与卡尔曼滤波相比，无迹卡尔曼滤波更加稳定，具有更好的适用性和精度。
3.扩展卡尔曼滤波扩展卡尔曼滤波是针对非线性系统而提出的一种卡尔曼滤波改进算法，它通过对非线性系统进行线性化，进而应用卡尔曼滤波的方法进行状态估计处理，其优点是能够在非线性系统中实现高精度的状态估计。
4.粒子滤波粒子滤波是一种全局搜索算法，它通过粒子集合对系统状态进行估计。
粒子滤波的主要特点是可以处理非线性、非高斯等复杂的状态估计问题。
与传统的基于概率密度的算法不同，粒子滤波是基于样本的方法，因此能够更好地适应复杂的状态估计。
5.互模糊滤波互模糊滤波是一种基于模糊集合理论的滤波算法，它通过融合多个传感器的信息，对系统的状态进行估计。
与传统的滤波算法相比，互模糊滤波在处理不确定性和噪声时更加有效，能够实现高精度的状态估计。
总的来说，状态估计算法在自动控制系统中发挥着重要的作用，实现高精度的状态估计将有助于提高自动化系统的控制性能和运行效率。
因此，在实际应用中，需要根据具体的应用场景来选择适合的状态估计算法，以实现最优的控制效果。
滚动时域估计与扩展卡尔曼滤波-概述说明以及解释
滚动时域估计与扩展卡尔曼滤波-概述说明以及解释1.引言1.1 概述概述：滚动时域估计是一种基于时间序列数据的估计方法，它能够实时地对数据进行连续的估计和预测。
相较于传统的批量估计方法，滚动时域估计具有更高的实时性和适应性，可以应用于各种领域的数据处理和分析中。
扩展卡尔曼滤波是一种常用的信号处理和状态估计方法，它可以应对非线性和非高斯的系统模型。
扩展卡尔曼滤波通过不断更新系统状态的估计值和协方差矩阵，能够较准确地估计系统的状态并实现滤波、预测和数据关联等功能。
本文将对滚动时域估计和扩展卡尔曼滤波进行详细讨论和比较。
首先，我们将介绍滚动时域估计的定义和原理，包括滚动窗口、递推估计和预测等关键概念。
然后，我们会探讨滚动时域估计的应用领域，例如信号处理、时间序列分析、金融数据预测等。
接着，我们会分析滚动时域估计的优势和挑战，包括实时性、适应性、计算复杂度等方面的考虑。
在扩展卡尔曼滤波部分，我们将介绍扩展卡尔曼滤波的基本原理，包括状态空间模型、观测方程、状态估计和协方差更新等关键过程。
然后，我们会探讨扩展卡尔曼滤波的应用场景，例如导航系统、机器人定位、传感器数据融合等。
最后，我们将介绍扩展卡尔曼滤波的改进和发展，包括无迹卡尔曼滤波、粒子滤波等扩展方法。
文章的结论部分将对滚动时域估计和扩展卡尔曼滤波进行比较和总结，讨论它们在实践中的优劣和适用性。
同时，我们将展望滚动时域估计和扩展卡尔曼滤波的未来发展方向，讨论它们在各个领域的应用前景和挑战。
通过本文的探讨和比较，读者将能够全面了解滚动时域估计和扩展卡尔曼滤波的原理、应用和发展趋势，为相关领域的研究和实践提供参考和指导。
1.2文章结构文章结构部分的内容可以包括以下内容：在本文中，我们将探讨滚动时域估计与扩展卡尔曼滤波两种方法的原理、应用领域、优势和挑战，以及它们之间的比较和应用前景。
文章将由以下几个部分组成：第一部分是引言部分。
在这一部分，我们将对整篇文章进行一个概述，介绍滚动时域估计和扩展卡尔曼滤波的基本概念，以及文章的目的和结构。
扩展卡尔曼滤波器原理
扩展卡尔曼滤波器原理一、引言扩展卡尔曼滤波器（Extended Kalman Filter，EKF）是一种常用的非线性滤波器，其原理是对非线性系统进行线性化处理，从而利用卡尔曼滤波器的优势进行状态估计和滤波。
本文将介绍扩展卡尔曼滤波器的原理及其应用。
二、卡尔曼滤波器简介卡尔曼滤波器是一种基于最优估计理论的滤波算法，广泛应用于估计系统状态。
卡尔曼滤波器通过对系统状态和观测数据进行加权平均，得到对系统状态的估计值。
其基本原理是通过系统的动力学方程和观测方程，利用贝叶斯概率理论计算系统状态的后验概率分布。
三、非线性系统的滤波问题在实际应用中，许多系统都是非线性的，而卡尔曼滤波器是基于线性系统模型的。
因此，当系统模型非线性时，传统的卡尔曼滤波器无法直接应用。
扩展卡尔曼滤波器就是为了解决这个问题而提出的。
四、扩展卡尔曼滤波器原理扩展卡尔曼滤波器通过对非线性系统进行线性化处理，将非线性系统转化为线性系统，然后利用卡尔曼滤波器进行状态估计。
其基本思想是通过一阶泰勒展开将非线性系统进行线性逼近。
具体步骤如下：1. 系统模型线性化：将非线性系统的动力学方程和观测方程在当前状态下进行一阶泰勒展开，得到线性化的系统模型。
2. 预测步骤：利用线性化的系统模型进行状态预测，得到预测的状态和协方差矩阵。
3. 更新步骤：利用观测方程得到的测量值与预测的状态进行比较，计算卡尔曼增益。
然后利用卡尔曼增益对预测的状态和协方差矩阵进行更新，得到最终的状态估计和协方差矩阵。
五、扩展卡尔曼滤波器的应用扩展卡尔曼滤波器广泛应用于各个领域，包括机器人导航、目标跟踪、航天器姿态估计等。
以机器人导航为例，机器人在未知环境中通过传感器获取的信息是非线性的，而机器人的运动模型也是非线性的。
因此，利用扩展卡尔曼滤波器可以对机器人的位置和姿态进行估计，从而实现导航功能。
六、总结扩展卡尔曼滤波器是一种处理非线性系统的滤波算法，通过对非线性系统进行线性化处理，利用卡尔曼滤波器进行状态估计和滤波。
卡尔曼滤波进行状态估计模型
卡尔曼滤波进行状态估计模型
卡尔曼滤波是一种用于估计系统状态的强大工具，它在许多领域都有着广泛的应用，包括航空航天、自动控制、金融领域等。
本文将介绍卡尔曼滤波的基本原理和应用，并探讨其在状态估计模型中的重要性。
首先，让我们了解一下卡尔曼滤波的基本原理。
卡尔曼滤波是一种递归的状态估计方法，它通过将系统的动态模型和测量模型结合起来，不断地更新对系统状态的估计。
卡尔曼滤波的核心思想是利用系统的动态模型来预测下一个时刻的状态，然后利用测量值来修正这一预测，从而得到对系统真实状态的更准确估计。
在实际应用中，卡尔曼滤波通常用于处理带有噪声的传感器数据，以及对系统状态进行估计。
例如，在飞行器导航系统中，卡尔曼滤波可以用来估计飞行器的位置和速度，从而实现精确的导航控制。
在自动驾驶汽车中，卡尔曼滤波可以用来融合来自不同传感器的数据，以实现对车辆位置和周围环境的准确估计。
除了在航空航天和自动控制领域的应用外，卡尔曼滤波在金融领域也有着重要的应用。
例如，它可以用来对金融市场的波动进行
预测，从而帮助投资者做出更明智的决策。
总之，卡尔曼滤波是一种强大的状态估计方法，它在许多领域
都有着广泛的应用。
通过结合系统动态模型和测量模型，卡尔曼滤
波可以对系统状态进行准确的估计，从而为实际应用提供了重要的
支持。
希望本文能够帮助读者更好地理解卡尔曼滤波的原理和应用，并在实际工程中加以应用。
卡尔曼滤波进行状态估计模型
卡尔曼滤波进行状态估计模型
卡尔曼滤波是一种用于状态估计的强大工具，它在许多现代科
学和工程领域中都得到了广泛的应用。
这种滤波器能够从一系列不
完全、噪声干扰的测量中，估计出系统的真实状态。
它的应用范围
包括但不限于航空航天、导航、无人机、自动控制系统和金融领域。
卡尔曼滤波的核心思想是通过将先验信息（系统的动态模型）
和测量信息（传感器测量）进行融合，来估计系统的真实状态。
它
能够有效地处理测量噪声和模型不确定性，并且能够提供对系统状
态的最优估计。
卡尔曼滤波的工作原理是通过不断地更新系统状态的估计值，
以使其与实际状态尽可能接近。
它通过两个主要步骤实现这一目标，预测和更新。
在预测步骤中，根据系统的动态模型和先验信息，估
计系统的下一个状态。
在更新步骤中，根据测量信息，修正先前的
状态估计，以获得最优的系统状态估计。
卡尔曼滤波的优势在于它能够在计算复杂度相对较低的情况下，提供对系统状态的最优估计。
它还能够有效地处理非线性系统，并
且能够适应不同类型的测量噪声。
总的来说，卡尔曼滤波是一种强大的状态估计工具，它在许多现代应用中都发挥着重要作用。
通过将先验信息和测量信息进行融合，它能够提供对系统状态的最优估计，为科学和工程领域的研究和应用提供了重要的支持。
基于三维扩展卡尔曼滤波的算法
基于三维扩展卡尔曼滤波的算法一、原理具体来说，基于三维扩展卡尔曼滤波的算法主要包括以下几个步骤：1.初始化：初始化状态向量和协方差矩阵。
2.预测：根据系统的动力学模型，预测当前时刻的状态向量和协方差矩阵。
3.估计：根据传感器的测量数据，通过观测模型更新状态向量和协方差矩阵。
4.线性化：通过对非线性系统进行线性化，将其转化为一系列线性系统，利用卡尔曼滤波的递推过程进行状态估计。
5.迭代：重复步骤2和步骤3，直到收敛或达到预设的迭代次数。
二、应用1.机器人感知和导航：基于三维扩展卡尔曼滤波的算法可以用于估计机器人的姿态、位置和速度等状态，从而实现机器人的自主定位和导航。
2.无线定位：基于三维扩展卡尔曼滤波的算法可以用于估计无线传感器节点的位置和速度等状态，通过无线通信信号的测量数据进行状态估计。
3.目标跟踪：基于三维扩展卡尔曼滤波的算法可以用于估计目标的位置、速度和姿态等状态，从而实现目标的跟踪和预测。
三、优缺点1.可以处理非线性系统：相比传统的卡尔曼滤波算法，基于三维扩展卡尔曼滤波的算法可以处理非线性系统，具有更广泛的应用范围。
2.可以结合不同的传感器数据：基于三维扩展卡尔曼滤波的算法可以通过观测模型将多个传感器的测量数据集成起来，实现更准确的状态估计。
3.可以实时更新状态：基于三维扩展卡尔曼滤波的算法可以根据当前的测量数据实时更新状态向量和协方差矩阵，实现实时的状态估计。
然而，基于三维扩展卡尔曼滤波的算法也存在一些缺点：1.对模型准确性要求较高：基于三维扩展卡尔曼滤波的算法对系统的模型准确性要求较高，如果系统的动力学模型不准确，可能导致状态估计结果不准确。
2.计算复杂度较高：基于三维扩展卡尔曼滤波的算法需要进行状态预测和观测更新的迭代过程，其计算复杂度较高，对计算资源要求较高。
总结起来，基于三维扩展卡尔曼滤波的算法是一种用于非线性系统状态估计的滤波算法，具有处理非线性系统和结合多个传感器数据的优点，但对模型准确性和计算资源要求较高。
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
legend('Δx','Δy');
xlabel('t');ylabel('Error');
plot(t,error(:,1),'b-x');
hold on;
plot(t,error(:,2),'r-');
x=x_kk1(1,1);
y=x_kk1(4,1);
h=x_kk1(7,1);
h_k=[(x^2+y^2+h^2)^(1/2);atan2(y,x);atan2(h,sqrt(x^2+y^2))];
H_k=[1/(x^2+y^2+h^2)^(1/2)*x 0 0 1/(x^2+y^2+h^2)^(1/2)*y 0 0 1/(x^2+y^2+h^2)^(1/2)*h 0 0;
error(i,3)=xk(i,7)-cpolreal(i,3);
end
t=0:0.2:149*0.2;
figure(1);hold on;
title('x-y曲线');
xlabel('Position x');ylabel('Position y');
legend('xk','cpolreal','cpol');
cpolreal(i,2)=polreal2(i,1)*cos(polreal2(i,3)/6000*2*pi)*sin(polreal2(i,2)/6000*2*pi);
cpolreal(i,3)=polreal2(i,1)*sin(polreal2(i,3)/6000*2*pi);
end %cpolreal直角坐标下的（单位弧度）
xk(1,4)=cpol(1,2);xk(1,5)=dy;xk(1,6)=dyy;
xk(1,7)=cpol(1,3);xk(1,8)=dh;xk(1,9)=dhh;
x0=xk(1,:);
p0=1000*eye(9);
x_k1k1=x0';
p_k1k1=p0;
for k=2:150;
yk=ppol(k,:);
gr=[TT^2/2 TT 1]';
tou=[gr zeros(3,1) zeros(3,1);zeros(3,1) gr zeros(3,1);zeros(3,1) zeros(3,1) gr];
R=[20 0 0;0 5*2*pi/6000 0;0 0 5*2*pi/6000];
x_kk1=F*x_k1k1;
plot(xk(:,1),xk(:,4),'r-v');hold on;
plot(cpolreal(:,1),cpon;
plot(cpol(:,1),cpol(:,2),'b-x');
t=0:0.2:149*0.2;
figure(2);hold on;
1）扩展卡尔曼的递推公式的程序
function [x_kk,p_kk]=KF(x_k1k1,p_k1k1,yk)
TT=0.2;
ztzy=[1 TT TT^2/2;0 1 TT;0 0 1];
F=[ztzy zeros(3,3) zeros(3,3);zeros(3,3) ztzy zeros(3,3);zeros(3,3) zeros(3,3) ztzy];
-y/x^2/(1+y^2/x^2) 0 0 1/x/(1+y^2/x^2) 0 0 0 0 0;
-h/(x^2+y^2)^(3/2)*x/(1+h^2/(x^2+y^2)) 0 0 -h/(x^2+y^2)^(3/2)*y/(1+h^2/(x^2+y^2)) 0 0 1/(x^2+y^2)^(1/2)/(1+h^2/(x^2+y^2)) 00];
p_kk1=F*p_k1k1*F'+0.1*tou*tou';
k_k=p_kk1*H_k'*inv(H_k*p_kk1*H_k'+R);
x_kk=x_kk1+k_k*(yk'-h_k);
p_kk=(eye(9)-k_k*H_k)*p_kk1;
return
2）主函数的程序
cd D:\MATLAB;
load('polreal2.dat');
cpol(i,3)=polcl2(i,1)*sin(polcl2(i,3)*2*pi/6000);
end %cpol直角坐标下的
for i=1:150
cpolreal(i,1)=polreal2(i,1)*cos(polreal2(i,2)/6000*2*pi)*cos(polreal2(i,3)/6000*2*pi);
for i=1:150
ppol(i,1)=polcl2(i,1);
ppol(i,2)=polcl2(i,2)*2*pi/6000;
ppol(i,3)=polcl2(i,3)*2*pi/6000;
end
T=0.2;
dx=(cpol(2,1)-cpol(1,1))/T;dxx=(cpol(3,1)+cpol(1,1)-2*cpol(2,1))/T^2;
dy=(cpol(2,2)-cpol(1,2))/T;dyy=(cpol(3,2)+cpol(1,2)-2*cpol(2,2))/T^2;
dh=(cpol(2,3)-cpol(1,3))/T;dhh=(cpol(3,3)+cpol(1,3)-2*cpol(2,3))/T^2;
xk(1,1)=cpol(1,1);xk(1,2)=dx;xk(1,3)=dxx;
load('polcl2.dat');
for i=1:150;
cpol(i,1)=polcl2(i,1)*cos(polcl2(i,3)*2*pi/6000)*cos(polcl2(i,2)*2*pi/6000);
cpol(i,2)=polcl2(i,1)*cos(polcl2(i,3)*2*pi/6000)*sin(polcl2(i,2)*2*pi/6000);
[x_kk,p_kk]=KF(x_k1k1,p_k1k1,yk);
xk(k,:)=x_kk';
x_k1k1=x_kk;
p_k1k1=p_kk;
end
for i=1:150;
error(i,1)=xk(i,1)-cpolreal(i,1);
error(i,2)=xk(i,4)-cpolreal(i,2);