运用于电能质量分析的卡尔曼滤波与仿真研究

1、运用于电能质量分析的卡尔曼滤波与仿真研究 姓名： 班级： 学号：运用于电能质量分析的卡尔曼滤波与仿真研究摘要：在现代电力系统中，由于新能源的接入，以及人们对电能质量的要求越来越高，电能质量分析变得越来越重要。这些变化使得系统状态参数中往往混杂着噪声，因此有必要采取适当的方法，从随机干扰的观测信号中提取有效的系统状态参数。现有运用于电能质量检测的方法有很多，但由于人们对滤波过程中的的数值稳定性、快速性、实用性和实用性提出越来越高的要求时，各种滤波方法的缺点越来越明显。在此基础上，卡尔曼滤波越来越受关注，并在此基础上的各种改进措施也越来越多。本文首先介绍卡尔曼滤波的基本原理，接着对运用于电能质量的

2、改进的扩展卡尔曼滤波以及无迹卡尔曼滤波进行介绍。在比较改进的两种卡尔曼滤波器的利弊后，进行仿真比较和验证，最后对卡尔曼滤波方法提出自己的一点理解与看法。关键词：电能质量，卡尔曼滤波，扩展卡尔曼滤波，无迹卡尔曼滤波0 引言 电能质量是指优质供电，合格的电能质量是指供给敏感设备的电力的相应的接地系统是适合该设备正常工作的。一个理想的三相交流电力系统应该具有恒定的频率(50Hz)和正弦的波形，A、B、C三相之间满足相位相差120度的关系。而由于大规模电力电子设备、新能源和其他非线性元件在电力系统中的应用，使得电能质量变得越来越差，引起诸多问题，例如：谐波，电压骤降，电压骤升，电压波动，电压中断，电压

3、切痕，脉冲瞬变等。在以上列出的电能质量问题中，谐波是电力系统中最普遍存在的问题。 美籍科学家卡尔曼在系统状态空间模型的基础上提出了著名的线性卡尔曼滤波器。这个系统可用包含正交状态变量的微分方程模型来描述，这种滤波器是将过去的测量估计误差合并到新的测量误差中来估计将来的误差。卡尔曼滤波器存在的问题是它的假设前提是线性系统，并在线性系统下设计的线性无偏、最小方差估计器。而电力系统是高度的非线性系统，故在此基础上人们提出了许多改进的卡尔曼滤波器方法，比如扩展卡尔曼滤波器(EKF)、无迹卡尔曼滤波器(UKF)、中心差分卡尔曼滤波器(CDKF)、粒子滤波器(PF)。另外，也有人将卡尔曼滤波器与其他方法结

4、合起来，例如将采用相位修正小波变换即S变换和扩展卡尔曼滤波相结合。 在电力系统中，卡尔曼也被应用于很多方面。在电力系统短期负荷预测方面，采用最小二乘支持向量机(RLS-SVM)技术和卡尔曼滤波技术分别对节点有功分配因子和节点功率因数建立自适应动态预测模型，结果表明两种算法满足了系统运算速度、鲁棒性和预测能力的要求；在电力系统动态状态估计方面，人们提出了混合卡尔曼粒子滤波器(MKPF)，该滤波器在电力系统在受到扰动之后，可以很好地收敛于真实值，和EKF和UKF相比具有更高的精度和稳定性；在电能质量分析方面，卡尔曼滤波尤其是扩展卡尔曼滤波和无迹卡尔曼滤波在频率跟踪领域得到了广泛的应用，在傅里叶变换

5、方法、最小绝对值状态估计、遗传算法、小波分析、Teager 能量算子等方法中脱颖而出；在电力系统继电保护方面，卡尔曼滤波具有响应速度快、滤波效果好的特点，能够满足继电保护快速跳闸和准确动作的要求，从而在电力系统继电保护中呈现出了很好的应用前景；在风电场风速预测中方面，通过卡尔曼滤波修正的风速数据能够很好地跟踪实际风速数据的变化趋势，预测精度得到了明显提高；在电机状态和参数估计方面，卡尔曼滤波在同步电机尤其是永磁同步电机(PMSM)中发挥了极致作用。 本文着重从卡尔曼滤波原理、分类及其在电能质量分析中的应用进展进行系统的介绍和分析，并对卡尔曼滤波器存在的可能研究的问题提出自己的看法。1 常规卡尔

6、曼滤波 根据经典物理学可知，在没有外部干扰时，一个系统未来的状态可根据目前的状态从已知的运动方程中确定出来。但任何实际的系统总是存在外部干扰，人们对其运动方程的描述不可能很准确。可以认为：任何实际物理系统的行为都由2部分组成，一部分是根据已知运动方程正确地预测出来，另一部分是均方为零的随机分量。 在分析期望响应存在的情况下的线性最优滤波器，我们由Wiener滤波器。在期望响应未知，线性最优滤波器即是Kalman滤波器。Kalman滤波理论是Wiener滤波理论的发展，它最早用于随机过程的参数估计，后来很快在各种最优滤波的最优控制问题中得到了极其广泛的应用。Kalman滤波器具有以下特点：(1)

7、其数学公式用状态空间概念描述；(2)它的解是递推计算的，级与Wiener滤波器不同，Kalman滤波器是一种自适应滤波器。下面对其进行简单介绍。 假设线性离散方程为 (1) (2)式中：为状态向量；为量测向量；为系统噪声向量；为量测噪声向量；为状态转移矩阵；为量测转移矩阵。假设系统噪声和量测噪声是互不相关的零均值高斯白噪声，方差阵分别为、，为了要递推地在每次获得观测量之后估计状态量。定义状态量的一步预测为，其他类推，则卡尔曼滤波递推方程如下。状态1步预测为 （3）1步预测误差方差阵为 (4)状态估计为 (5)估计误差方差阵为 (6)滤波增益矩阵为 (7)式中为单位阵。式(3)(7)就是随机线性

8、离散系统卡尔曼滤波的基本方程。只要给定初值和及、，根据时刻的观测值就可以递推计算得时刻的状态估计（=1,2，···）。卡尔曼滤波有如下特点： 由于Kalman滤波算法将被估计的信号看作在白噪声作用下的一个随机线性系统的输出,并且其输入输出关系是由状态方程和输出方程在时间域内给出的,因此这种滤波方法仅适用于平稳序列的滤波,而且特别适用于非平稳或平稳马尔可夫序列或高斯一马尔可夫序列的滤波,因此其应用范围是十分广泛的。 由于滤波的基本方程是时间域内的递推形式,其计算过程是一个不断的“预测一修正”过程,在求解时不要求存储大量数据,并且一旦观测到了新的数据,随时可以算得新的

9、滤波值,因此这种滤波方法非常便于实时处理和计算机实现。 由于滤波器的增益矩阵与观测无关,因此它可预先离线算出,从而可以减少实时在线计算量;在求滤波器增益矩阵,时,要求一个矩阵的逆,即要计算，它的阶数只取决于观测方程的维数m,而m通常是很小的。这样,上面的求逆运算是比较方便的;另外,在求解滤波器增益的过程中, 随时可以算得滤波器的精度指标,其对角线上的元素就是滤波误差向量各分量的方差。 增益矩阵与初始方差阵、系统噪声方差阵、以及观测噪声方差阵之间具有如下关系：·，和同乘一个相同的标量时，值不变。·当增大时, 就变小,这在直观上是很容易理解的，因为如果观测噪声增大,那么滤波增益

10、就应取小一些（因为这时的新信息里的误差比较大），以减弱观测噪声对滤波值的影响·如果变小, 变小,或两者都变小，则变小，也变小，从而变小。这也是很自然的。因为变小,表示初始估计较好, 变小,表示系统噪声变小,于是增益矩阵也应小些以便给于较小的修正。综上所述,可以简单地说,增益矩阵与成正比,而与成反比。通过仿真附录一中的卡尔曼滤波实验程序，我们可得如下图形:. 和的图： . 真实值与估计值的比较图 . 各自的误差图 .通过用卡尔曼滤波器的状态误差协方差矩阵画出的和：分析：中的方差是中的误差平方后取均值，是均方误差。误差直接由真实值减去估计值，有正有负，而均方误差没有这个缺陷，更能综合的表

11、示滤波的效果。由图(2)可知，从真实值与估计值的比较可以看出，卡尔曼滤波器还是能够有效的跟踪真实值，但是由图(3)明显看出，在跟踪的过程中，误差在零值上下摆动，存在不稳定的问题。真实值与估计值之间的误差幅值一直处于抖动状态，由此，我们可以得出，运用卡尔曼滤波，虽然能够实现运用较少的信息量实现平稳与非平稳过程的追踪，但也有当运动目标长时间被遮挡时会存在目标跟踪丢失的情况 。2. 扩展卡尔曼滤波由于在实际中广泛存在的是非线性状态空间模型，使得常规卡尔曼滤波在电能质量分析中的应用存在困难，于是便出现了诸多针对非线性模型的次优方法，其中应用最广泛的是扩展卡尔曼滤波(extended Kalman fi

12、ltering，EKF)。EKF 是将非线性系统线性化，与线性卡尔曼滤波公式完全类似。其主要思想是对非线性函数的泰勒展开式进行截断，实现非线性函数的线性化。根据泰勒展开式进行的是1阶还是2阶截取，EKF主要分为1阶EKF(first order EKF)和2阶EKF(second order EKF)。电能质量分析中最常用的是1阶EKF，原理简述如下。假如非线性系统可表示为 （8） （9）式中： 为系统状态向量；为系统量测向量；和 是关于状态的非线性函数； 和 均是均值为零的高斯白噪声。式(8)、(9)分别是状态方程和量测方程。为了使卡尔曼滤波应用到非线性系统中，非线性系统必须在指定位置进行泰

13、勒展开，实现线性化。推导过程如下：利用泰勒公式，分别在和处对状态方程和观测方程进行1阶泰勒展开，可得 （10） （11）假设 （12） （13） （14） （15）则式(10)、(11)可改写成与常规卡尔曼方程相似的形式： （16） （17）1阶EKF递推方程组与常规卡尔曼滤波递推方程组在形式上相同，不同的是：中的 和 被 1阶EKF中的Jacobian矩阵和代替，并且预测平均值和预测的冗余在EKF中也分别计算，其递推方程与卡尔曼滤波相同。在电能质量分析中，、 矩阵的设计略有不同。1991 年，Beides H M 和Heydt G T 提出用扩展卡尔曼滤波获得电力系统谐波的动态状态估计，经过

14、实验室仿真和实测试验证明扩展卡尔曼滤波能动态地追踪谐波内容和时间。1993 年，Kamwa也将 EKF引入电力系统电能质量分析中，用于测量闪变。基于卡尔曼滤波的实时监控电压闪变的算法，建立的模型是具有线性时不变过渡矩阵的系统电压随机状态空间，用 EKF来测量带噪声的单相电压。一种测量 50/60Hz低频信号调制的卡尔曼滤波方法，用于确定信号和随机信号的调制，产生的参数将电压闪变控制在一个可接受的范围内，这种方法运用了扩展卡尔曼滤波模型，经过典型事例的试验证明了其测量电压闪变的准确性和可靠性，并具有预测电压闪变的功能。一种基于 13状态系统的电压暂降的扩展卡尔曼滤波检测和分析方法，实验室仿真和实

15、际电压暂降仿真表明，这种方法效果明显优于IEC 电能质量标准的RMS方法。虽然扩展卡尔曼滤波有很好的发展前景，但它在实际应用中存在明显的缺陷：一是线性化有可能产生极不稳定的滤波；二是EKF需要计算Jacobian矩阵的导数，实现起来较为复杂，而对于一些不可微的情况，EKF可能失效。在模型非线性较强以及系统噪声非高斯时，估计的精度严重降低，甚至会造成滤波器发散。近几年，更多的人针对EKF对谐波的跟踪和检测提出了更好的改进方法，其中最重要的是扩展复卡尔曼滤波器或复数型扩展卡尔曼滤波(extended complex Kalman filter，ECKF)。一种用于估计混有噪声的信号频率的ECKF。

16、由于复数滤波器公式需要多传感器来测量三相信号，通过重置由输出误差幅度决定的协方差矩阵Q，使设计的模型仿真不仅考虑了频率、幅值和相位的测量，也考虑到了实际复杂的网络情况，实验结果表明其性能优于普通的扩展卡尔曼滤波器。一种 PEKF(the proposed variant of the extended Kalman filtering)用于测量电力系统谐波，并在数学上证明了其稳定性，与 ADALINE (adaptive linear combiner)、ANF(adaptive notch filter)、MFT(multi frequency tracker)以及现有的常规EKF(ordi

17、nary extended Kalman filter，OEKF)和ECKF 相比，试验表明其具有更好的性能和更小的计算量，更适合实时实现。Huang 提出一种基于ECKF 的鲁棒性算法来提高电力系统的频率估计效果，利用在压缩数据中产生的不良数据来提高频率估计效率，并与ECKF算法进行比较，证明其提高了系统执行效率。3. 无迹卡尔曼滤波 为了更精确地拟合非线性函数，Julier 提出了(unscented transformation，UT)和无迹卡尔曼滤波(unscented Kalman filtering，UKF)。通过结合无迹变换和无迹卡尔曼滤波来实现非线性系统的状态估计，是近年来用于

18、解决该问题的一种新的热点方法。它通过一组精确选择的 Sigma 点来匹配随机量的统计特性，UKF 没有涉及非线性映射函数的Jacobian矩阵计算问题，从而使算法的实现EKF更为容易, 在保持相当运算量的同时，具有更高的估计精度和更广泛的适用范围。传统的线性化方法是对非线性映射本身做某种线性近似，然后再应用线性估计的各种方法。而Julier S. J.等人提出的无迹变换则是基于用有限的参数来近似随机量的概率统计特性要比近似任意的非线性映射函数更为容易的思想。无迹变换的基本步骤可概括为：关于 的Sigma点集的产生不确定性的非线性变换与传递关于的统计特性的推算。无迹变换 Sigma点集的选取方式

19、不同，会产生很多种变换的演变形式，其目的主要是进一步提高变换的精度，增强算法的稳定性和减小运算量等。在应用UKF时首先要对状态量进行扩展，也就是将模型噪声也作为状态量的一部分，相应地，无迹变换中用到的Sigma点也需要扩展，具体表示如下。扩展状态方程的初始值： （16） （17）式中：为模型初始状态变量；和 分别为扩展状态向量的均值和协方差阵。Sigma点集的创建通过下式实现： （18）式中：为预测空间维数；表示矩阵平方根的第 个行向量或列向量，而矩阵平方根的常见求法是采用Cholesky 分解；为点处的协方差；，决定Sigma取的点数，是由和参数决定的函数，为控制周围的高阶非线性值的参数，是

20、介于 0.00011之间的一个常数，是次要的比例参数，通常设置为 0 或3n ，以确保Sigma点分布的峭度与高斯分布的峭度一致。Sigma点向量通过状态方程的非线性影射得到： （19）扩展状态量的1步预测为 （20）扩展状态量1步预测的协方差阵为 （21）然后计算量测空间。量测空间的Sigma点集的创建通过下式实现： （22）1 步预测的扩展状态Sigma点向量经过观测方程的非线性映射得到： （23）观测量的1步预测为 （24）式中：；为合并高阶状态分布的先验知识，高斯分布的最佳选择是2。 观测量1步预测的协方差阵为 （25）1 步预测的互协方差阵为 （26）最后得到量测更新过程。更新误差协

21、方差： （27）计算滤波增益矩阵： （28）更新估计： （29）UKF在保持相当运算量的同时具有更高的估计精度和更为广泛的适用范围，它在国内的相关研究起步较晚，但发展很快。可查的公开资料主要集中于最近的23年内，而且在电力系统尤其是电能质量方面的研究成果比较少。电能质量幅度和频率估计方面比较了无迹卡尔曼滤波和扩展卡尔曼滤波的差异。仿真试验表明，在相等的噪声条件下，无迹卡尔曼滤波与扩展卡尔曼滤波的计算精度相似，但无迹卡尔曼滤波的计算量更小。罗谌持、张明应用无迹变换，将复数型Sigma点卡尔曼滤波(complex Sigma point Kalman filtering，CSPKF)中的扩展复卡尔

22、曼滤波算法进行比较，通过变换，首先将三相电压信号转换成复电压信号，再利用CSPKF算法对发生谐波畸变和随机噪声干扰的电力系统电压信号的频率进行动态估计和跟踪的过程进行改进。算法仿真表明，CSPKF 算法具有优异的动态跟踪性能，迅速跟踪频率和幅值变化的同时又保持了较低的跟踪误差。4. 应用现状随着卡尔曼滤波的快速发展，除了上述3种形式及其改进算法以外，还出现了其他一些形式，例如集合变换卡尔曼滤波(the ensemble transform Kalman filtering，ETKF)也在现代工程应用中发挥了越来越重要的作用。就近十几年的发展而言，卡尔曼滤波在电能质量中的主流发展主要集中于对 E

23、KF 算法进行改进、UKF 的改进算法研究以及与其他方法相结合进行电能质量分析。方程中一些初始量(如噪声协方差矩阵 Q、滤波增益矩阵 K 等)的选择关系到动态追踪特性。一种自适应卡尔曼滤波，该方法的状态模型是线性的，频率作为状态变量与谐波母线电压独立。它利用统计规则在2个基本噪声协方差矩阵Q 模式间进行切换，取代了直接优化的Q模式来进行动态谐波状态估计。采用这种方法可以重置卡尔曼滤波增益K，从而更好地在暂态过程中实现快速追踪，并在新西兰220k V电网上进行仿真，证明了方法的可行性。一种卡尔曼滤波的自校正方法来测量谐波，并与噪声协方差矩阵 Q 为定值零的方法进行比较，证明了其追踪信号波动的优越

24、性。 除了单独采用卡尔曼滤波进行电能质量分析外，也有人提出将其与其他方法相结合来检测电能质量扰动。一种新的检测、定位、分类短期扰动的方法，采用相位修正小波变换即S 变换和扩展卡尔曼滤波相结合。S 变换有很好的时频特性，提供适合于电力系统自动识别的检测、定位和视觉识别，而EKF提供频繁出现的频率短期扰动的自动分类和测量。实验证明，结合ST和EKF可以完整地分类和测量电能质量短期扰动，在有噪声的情况下具有很高的精度。小波与卡尔曼滤波相结合的方法来自动检测和分析电压暂降。小波用来检测电压波形和更好地估算电压骤降的时间相关的参数，而卡尔曼滤波在这一过程中用来估计电压幅值和相位角，这种方法可用来处理实时

25、系统，并且已通过计算机仿真和实验对低压配电系统的测试，证明了其测量电压暂降的完整性和准确性。Kumar A、Das B 和Sharma J 提出了鲁棒扩展卡尔曼滤波(robust extended Kalman filtering，REKF)技术来进行动态谐波状态估计，人工神经网络和改进的EKF相结合的方法，人工神经网络用于测量谐波仪的位置和提供虚拟测量(pseudo-measurement)的粗略估计，REKF 用于进行动态谐波状态估计。文中通过正常负荷 情况、负荷突然变化和存在不良数据的IEEE-14 系统测量这3组实验，将REKF与EKF进行比较，证明REKF的性能优于EKF。状态估计器

26、即使在存在不良数据时也能实时估计电力系统谐波状态，从IEEE-14节点系统仿真结果来看，估计结果与真值相近。近年来，无迹变换和无迹卡尔曼滤波的研究取得了一些进展。无迹变换主要有：对称 Sigma点集无迹变换(symmetric Sigma set UT)、单形 Sigma点集无迹变换(simplex Sigma set UT)、变尺度无迹变换(scaled UT)、高阶无迹变换(high order UT)等。可以根据具体的场合提出特定分布的无迹变换的衍变形式，当然，通常只能根据特定的场合达到其中的部分目标。也可引入代价函数或惩罚函数来尽可能满足某种条件而不要求一定能够达到。考虑到提高算法的数

27、值稳定性和减小算法运算量，Merwe 和Wan借用平方根滤波(square-root filtering)，提出应用QR分解和Cholesky分解更新的方法直接非线性传递更新协方差阵的平方根，命名为平方根UKF(square-root UKF，SR-UKF)。5.实验验证与分析 通过附录二的程序，我们可以得到如下图形：(5) EKF、UKF估计值与真实值比较 由图可以看出，扩展卡尔曼滤波器和无迹卡尔曼滤波器都能跟踪真实值，且都存在一定的跟踪误差，从两者的整体趋势来看，无迹卡尔曼除了在40步长等少数点误差较大外，其余的跟踪点都比较准确，所以无迹卡尔曼具有比扩展卡尔曼跟优良的跟踪性能，与前文理论部

28、分符合。(6) EKF估计值与真实值及95%置信区间比较 在此，我们单独将EKF估计值与真实值和95%置信区间相比较，由此可以看出，虽然EKF大致能跟踪实际真实值，但明显有很大一部分数据没有跟踪到，存在一定误差。这里，虽然将误差放宽到95%，只有少部分点在误差区间，还有很大一部分点在置信区间外，说明，在卡尔曼滤波器基础上改进的EKF滤波器，仍然存在误差较大问题，虽然已经能运用在电能质量这种非线性系统中。(7) UKF估计值与真实值及95%置信区间比较 图(7)中，我们可以看出，相比于图(6)，UKF有明显的改善。UKF不仅能跟踪非线性系统，而且在此图中，只有几个点在95%置信区间外，并且都偏离

29、置信区间曲线不远。由此，我们可以得出结论，UKF比EKF有着跟好的滤波性能。(8) EKF与UKF估计值的误差比较 经过图(6)和图(7)的比较，我们能够得出初步的结论，这里，在图(8)，我们将EKF和UKF的估计值误差摆放在一起，我们可以更好地得出我们的结论：UKF比EKF在电能质量分析中拥有更好的性能。6. 结论 卡尔曼滤波是由卡尔曼在 20世纪60年代提出的基于贝叶斯滤波原理，用一个状态方程和一个量测方程来完整地描述线性动态过程的方法。扩展卡尔曼滤波算法中传统的解决方法是借助 Taylor 级数展开，其后也有很多改进算法。而无迹变换和无迹卡尔曼滤波是通过一组精确选择的 Sigma 点来匹

30、配随机量的统计特性，易于实现，目前虽取得了一些理论成果，但工程应用较少。因此，卡尔曼滤波的理论研究和分析方法仍有发展空间。 1) 算法的运算量和估算精度的平衡。影响算法运算量的一个重要因素是 Sigma点集中 Sigma点的数目，而估算精度是UT 和UKF的主要优势，如何平衡最合适的Sigma点数目很重要。2) 算法中参数的智能选取问题。UT 中涉及到的很多智能参数很有灵活性，其选取影响到程序的稳定性，甚至导致程序崩溃。因此，智能参数的选取显得至关重要。3) 算法的实际应用问题。针对电能质量中信号处理的一些特点，可以针对 UT 和 UKF 进行一些改进，而如何改进也显得十分重要。4) 保证卡尔

31、曼滤波算法最优性的一个先决条件是建立准确的系统方程和量测方程，这要求清晰了解系统的模型。偏离理想的系统模型或者动态模型误差在滤波时未模型化，这都必然会对滤波结果造成影响。另外，如何检测并修复模型误差，以使模型最佳。这都仍有待进一步的研究。5) 保证卡尔曼滤波算法最优性的另一个先决条件是正确描述系统噪声和量测噪声的统计特性。卡尔曼算法中要求两噪声都是白噪声且互不相关，但在实际应用中，系统噪声和量测噪声极有可能不是白噪声或二者相关。即有色噪声或者相关噪声条件下的卡尔曼滤波算法是可以继续加深研究的热点。附录一 卡尔曼滤波改进程序%卡尔曼滤波实验程序clc;y1=3.29691969,3.387365

32、15,7.02830641,9.71212521,11.42018315,15.97870583,22.06934285,28.30212781,30.44683831,38.75875595; %观测值y1(k) y2=2.10134294,0.47540797,3.17688898,2.49811140,2.91992424,6.17307616,5.42519274,3.05365741,5.98051141,4.51016361; %观测值y2(k) p0=1,0;0,1;p=p0; %均方误差阵赋初值Ak=1,1;0,1; %转移矩阵Qk=1,0;0,1; %系统噪声矩阵Ck=1,0

33、;0,1; %量测矩阵Rk=1,0;0,2; %测量噪声矩阵x0=0,0'xk=x0; %状态矩阵赋初值for k=1:10 Pk=Ak*p*Ak'+Qk; %滤波方程3 Hk=Pk*Ck'*inv(Ck*Pk*Ck'+Rk); %滤波方程2 yk=y1(k);y2(k); %观测值xk=Ak*xk+Hk*(yk-Ck*Ak*xk); %滤波方程1 x1(k)=xk(1); x2(k)=xk(2); %记录估计值p=(eye(2)-Hk*Ck)*Pk; %滤波方程4 pk(:,k)=p(1,1),p(2,2)' %记录状态误差协方差矩阵end figur

34、e %画图表示状态矢量的估计值subplot(2,1,1) i=1:10; plot(i,x1(i),'k') h=legend('x1(k)的估计值') set(h,'interpreter','none') subplot(2,1,2) i=1:10; plot(i,x2(i),'k') h=legend('x2(k)的估计值') set(h,'interpreter','none') X1=0,1.65428714,3.50300702,5.997852924,

35、9.15040740,12.50873910,16.92192594,21.34483352,25.89335144,31.54135330,36.93605670; %由模拟得到的实际状态值X1(k) X2=0,1.65428714,1.84871988,2.47552222,3.17187816,3.35833170,4.41318684,4.42290758,4.54851792,5.64800186,5.394470340; %由模拟得到的实际状态值X2(k) figure %在同一幅图中画出状态矢量的估计值与真实值subplot(2,1,1) i=1:10; plot(i,x1(i)

36、,'k',i,X1(i+1),'b') h=legend('x1(k)的估计值','x1(k)的真实值') set(h,'interpreter','none') subplot(2,1,2) i=1:10; plot(i,x2(i),'k',i,X2(i+1),'b') h=legend('x2(k)的估计值','x2(k)的真实值') set(h,'interpreter','none') for i

37、=1:10 %计算x(k)的误差e1(i)=X1(i+1)-x1(i); e2(i)=X2(i+1)-x2(i); end figure %画出误差图subplot(2,1,1) i=1:10; plot(i,e1(i),'r') h=legend('x1(k)的误差') set(h,'interpreter','none') subplot(2,1,2) i=1:10; plot(i,e2(i),'r') h=legend('x2(k)的误差') set(h,'interpreter

38、9;,'none') figure %通过用尔曼滤波器的状态误差协方差矩阵画出E1(k/k)2和E2(k/k)2i=1:10; subplot(2,1,1) plot(i,pk(1,i),'r') h= legend('由状态误差协方差矩阵得到的E1(k/k)2')set(h,'Interpreter','none') subplot(2,1,2) plot(i,pk(2,i),'r') h= legend('由状态误差协方差矩阵得到的E2(k/k)2')set(h,'Int

39、erpreter','none') 附录二% EKF UKF 的两个算法的仿真与比较clear;% tic;x = 0.1; % 初始状态 x_estimate = 1;%状态的估计e_x_estimate = x_estimate; %EKF的初始估计u_x_estimate = x_estimate; %UKF的初始估计Q = 10;%input('请输入过程噪声方差Q的值: '); % 过程状态协方差 R = 1;%input('请输入测量噪声方差R的值: '); % 测量噪声协方差 P =5;%初始估计方差e_P = P; %EK

40、F方差u_P = P;%UKF方差tf = 50; % 模拟长度 x_array = x;%真实值数组e_x_estimate_array = e_x_estimate;%EKF最优估计值数组u_x_estimate_array = u_x_estimate;%UKF最优估计值数组u_k = 1; %微调参数u_symmetry_number = 4; % 对称的点的个数u_total_number = 2 * u_symmetry_number + 1; %总的采样点的个数linear = 0.5;close all;for k = 1 : tf % 模拟系统 x = linear * x

41、+ (25 * x / (1 + x2) + 8 * cos(1.2*(k-1) + sqrt(Q) * randn; %状态值 y = (x2 / 20) + sqrt(R) * randn; %观测值 %扩展卡尔曼滤波器 %进行估计 第一阶段的估计 e_x_estimate_1 = linear * e_x_estimate + 25 * e_x_estimate /(1+e_x_estimate2) + 8 * cos(1.2*(k-1); e_y_estimate = (e_x_estimate_1)2/20; %这是根据k=1时估计值为1得到的观测值；只是这个由我估计得到的 第24行

42、的y也是观测值 不过是由加了噪声的真实值得到的 %相关矩阵 e_A = linear + 25 * (1-e_x_estimate2)/(1+e_x_estimate2)2);%传递矩阵 e_H = e_x_estimate_1/10; %观测矩阵 %估计的误差 e_p_estimate = e_A * e_P * e_A' + Q; %扩展卡尔曼增益 e_K = e_p_estimate * e_H'/(e_H * e_p_estimate * e_H' + R); %进行估计值的更新 第二阶段 e_x_estimate_2 = e_x_estimate_1 + e_

43、K * (y - e_y_estimate); %更新后的估计值的误差 e_p_estimate_update = e_p_estimate - e_K * e_H * e_p_estimate; %进入下一次迭代的参数变化 e_P = e_p_estimate_update; e_x_estimate = e_x_estimate_2; %采样点的选取 存在x(i) u_x_par = u_x_estimate; for i = 2 : (u_symmetry_number+1) u_x_par(i,:) = u_x_estimate + sqrt(u_symmetry_number+u_k

44、) * u_P); end for i = (u_symmetry_number+2) : u_total_number u_x_par(i,:) = u_x_estimate - sqrt(u_symmetry_number+u_k) * u_P); end %计算权值 u_w_1 = u_k/(u_symmetry_number+u_k); u_w_N1 = 1/(2 * (u_symmetry_number+u_k); %把这些粒子通过传递方程 得到下一个状态 for i = 1: u_total_number u_x_par(i) = 0.5 * u_x_par(i) + 25 * u

45、_x_par(i)/(1+u_x_par(i)2) + 8 * cos(1.2*(k-1); end %传递后的均值和方差 u_x_next = u_w_1 * u_x_par(1); for i = 2 : u_total_number u_x_next = u_x_next + u_w_N1 * u_x_par(i); end u_p_next = Q + u_w_1 * (u_x_par(1)-u_x_next) * (u_x_par(1)-u_x_next)' for i = 2 : u_total_number u_p_next = u_p_next + u_w_N1 * (

46、u_x_par(i)-u_x_next) * (u_x_par(i)-u_x_next)' end % %对传递后的均值和方差进行采样 产生粒子 存在y(i)% u_y_2obser(1) = u_x_next;% for i = 2 : (u_symmetry_number+1)% u_y_2obser(i,:) = u_x_next + sqrt(u_symmetry_number+k) * u_p_next);% end% for i = (u_symmetry_number + 2) : u_total_number% u_y_2obser(i,:) = u_x_next -

47、sqrt(u_symmetry_number+u_k) * u_p_next);% end%另外存在y_2obser(i) 中； for i = 1 :u_total_number u_y_2obser(i,:) = u_x_par(i); end %通过观测方程 得到一系列的粒子 for i = 1: u_total_number u_y_2obser(i) = u_y_2obser(i)2/20; end %通过观测方程后的均值 y_obse u_y_obse = u_w_1 * u_y_2obser(1); for i = 2 : u_total_number u_y_obse = u_

48、y_obse + u_w_N1 * u_y_2obser(i); end %Pzz测量方差矩阵 u_pzz = R + u_w_1 * (u_y_2obser(1)-u_y_obse)*(u_y_2obser(1)-u_y_obse)' for i = 2 : u_total_number u_pzz = u_pzz + u_w_N1 * (u_y_2obser(i) - u_y_obse)*(u_y_2obser(i) - u_y_obse)' end %Pxz状态向量与测量值的协方差矩阵 u_pxz = u_w_1 * (u_x_par(1) - u_x_next)* (u

49、_y_2obser(1)-u_y_obse)' for i = 2 : u_total_number u_pxz = u_pxz + u_w_N1 * (u_x_par(i) - u_x_next) * (u_y_2obser(i)- u_y_obse)' end %卡尔曼增益 u_K = u_pxz/u_pzz; %估计量的更新 u_x_next_optimal = u_x_next + u_K * (y - u_y_obse);%第一步的估计值 + 修正值； u_x_estimate = u_x_next_optimal; %方差的更新 u_p_next_update =

50、u_p_next - u_K * u_pzz * u_K' u_P = u_p_next_update; %进行画图程序 x_array = x_array,x; e_x_estimate_array = e_x_estimate_array,e_x_estimate; u_x_estimate_array = u_x_estimate_array,u_x_estimate; e_error(k,:) = abs(x_array(k)-e_x_estimate_array(k); u_error(k,:) = abs(x_array(k)-u_x_estimate_array(k); end t = 0 : tf; f