第4讲：UKF滤波算法

弹箭与制导学报Journal of Projectiles, Rockets, Missiles and Guidance收稿日期：UKF滤波算法在弹箭落点估计中的应用曾凡桥1，陈国光1，高小东2，刘霖1（1中北大学机电工程学院，太原 030051；2豫西工业集团军品研发中心，河南南阳 473000）摘要：依据部分弹道的弹道轨迹测量数据，准确地预报弹道落点，是弹道修正弹药智能化的关键技术。

综合考虑滤波精度和算法复杂度，本文采用质点弹道模型和无迹卡尔曼滤波（Unscented Kalman Filter）方法，建立了弹道滤波模型和外推弹道模型。

通过对雷达测量数据的处理结果表明，UKF弹道滤波模型和质点外推模型有较高的精度。

关键词：UKF；弹道落点；雷达；弹道滤波中图分类号：TJ410.1 文献标志码：AApplication of Unscented Kalman Filter In Estimationof Projectile Falling PointZENG Fanqiao1，CHEN Guoguang1，GAO Xiaodong2，LIU Lin1（1 College of Mechatronic Engineering，North University of China，Taiyuan 030051，China；2Yuxi Industries Group Co.Ltd，Henan Nanyang 473000，China）Abstract: According to the measured data of part of ballistic trajectory to accurately predict the trajectory falling point ,it is the key technology of the intelligence of trajectory correction ammunition. Considering the filtering accuracy and complexity of algorithms , the ballistic model of filtering and the ballistic model of extrapolating were established by using particle trajectory equations and Unscented Kalman Filter method in this paper. The processing results of the measured data obtained by radar show that the ballistic model of UKF filtering and the particle ballistic model of extrapolating have higher accuracy.Keywords:Unscented Kalman Filter；trajectory falling point；radar；trajectory filtering0 引言在现代战争中，为减少不必要的附加损伤，要求弹箭具有精确的点目标打击能力，弹道修正弹就是诸多新型弹药中低成本、高精度的炮兵常规弹药之一。

ukf滤波算法范文UKF（Unscented Kalman Filter）是一种基于卡尔曼滤波的非线性滤波算法。

相比于传统的扩展卡尔曼滤波（EKF），UKF通过一种更好的方法来近似非线性系统的概率分布，从而提高了非线性滤波的精确度和鲁棒性。

UKF通过一种称为“无气味变换（unscented transform）”的方法来处理非线性函数。

该方法基于对概率分布的均值和协方差进行一系列的采样点选择，然后通过变换这些采样点来近似非线性函数的传播。

这些采样点被称为“Sigma点”，可以看作是真实系统状态在均值周围的一系列假设状态。

UKF的基本步骤如下：1.初始化：初始化系统状态和协方差矩阵。

2. 预测步骤（Prediction）：- 通过生成Sigma点来近似系统状态的概率分布。

- 将Sigma点通过非线性函数进行变换，得到预测状态和预测协方差矩阵。

-计算预测状态的均值和协方差。

3. 更新步骤（Update）：- 通过生成Sigma点来近似测量函数的概率分布。

- 将预测状态的Sigma点通过测量函数进行变换，得到预测测量和预测测量协方差矩阵。

-计算预测测量的均值和协方差。

-根据实际测量值和预测测量的概率分布，计算卡尔曼增益。

-更新预测状态和协方差。

UKF相比于EKF具有以下优势：1.不需要对非线性函数进行线性化。

EKF通过一阶泰勒展开来线性化非线性函数，这可能导致误差积累和不稳定性。

UKF通过采样点直接逼近非线性函数，避免了这个问题。

2.更好的估计准确度和收敛性。

UKF通过采样点的选择更好地逼近了真实概率分布，提高了滤波的准确度和收敛性。

3.适用于高维状态空间。

EKF在高维状态空间中存在计算复杂度高和数值不稳定的问题，而UKF则通过更好的采样点选择来解决了这个问题。

4.对初始条件不敏感。

UKF对初始条件的选择不太敏感，可以在一定程度上避免初始条件选择不当导致的滤波失效问题。

尽管UKF相比于EKF有许多优势，但它也存在一些缺点。

UKF 算法滤波性能分析高海南 3110038011一、仿真问题描述考虑一个在二维平面x-y 内运动的质点M ，其在某一时刻k 的位置、速度和M 在水平方向（x ）作近似匀加速直线运动，垂直方向（y ）上亦作近似匀加速直线运动。

两方向上运动具其中假设一坐标位置为（0,0）的雷达对M显然在笛卡尔坐标系下，该模型运动观测方程为非线性的。

我们根据雷达测量值使用UKF 算法对目标进行跟踪，并与EKF 算法结果进行比较。

二、问题分析1. UKF 滤波跟踪有协方差阵k R 。

ukf 算法步骤如下： (1) 计算σ点1|1i k k --ξ，依据1|1k k --x 和1|1k k --p 生成2n+1个σ点1|1i k k --ξ，0,1,2,2i n =。

在UT 变换时，取尺度参数01.0=α，0=κ，2=β。

(2) 计算σ点|ik k ξ，即()()|1|12|1|02|1||1||110(),0,1,2,2ˆˆˆi i k kk k k nm i k k i k k i n T c i ik k i k k k k k k k k k i i n ωωω---=----=⎧⎪==⎪⎪=⎨⎪⎪=--+⎪⎩∑∑ξf ξxξp ξx ξx Q (3) 计算σ点|1ˆk k -x|1k k -p 通过量测方程对k x 的传播，即(4) 计算输出的一步提前预测，即(5) 获得新的量测后，进行滤波更新：2. 扩展卡尔曼滤波算法分析(1)()()()(())()k k k k k k k k +=+⎧⎨=+⎩x f x w z h x v 的，定义k |1k ˆ=()k k k k-∂=∂x x xf x f xEKF 算法步骤如下：k 时刻的一步提前预测状态预测误差协方差阵为卡尔曼滤波增益为在k 时刻得到新的量测后，状态滤波的更新公式为状态滤波协方差矩阵为三、实验仿真与结果分析N=50，采样时间为t=0.5。

迹如图1所示。

7.3 UKF 滤波UKF 滤波，仍然采用高斯分布代替状态量的分布，不同的是，通过特别挑选的样本点来表示状态量。

这些样本点完全可以表示出GRV （高斯随机变量）的均值和方差。

即使经过非线性变换（任何非线性变换）后，也能逼近变换后的均值和方差，逼近精度可以达到二阶泰勒展开后的精度。

下面开始介绍UT 变换。

UT 变换：UT 变换是计算随机变量经过非线性变换后的统计特征的一种方法。

考虑非线性函数：(x)y f =，记x 的均值和方差为,x x P 。

现在需要计算y 的统计特性，即ˆ,y yP 。

可以采用下面的方法，构造一个矩阵，矩阵中包含21L +个simga 向量：0 1,,, 1,,2,i i i i L xx i L x i L L -ℵ=ℵ=+=ℵ=-=+ (1)其中：2()L L λακ=+-——为尺度参数；α——确定sigma 点在x 周围的分布范围，4101α-≤≤； κ——尺度参数，其值通常为3L -β——x 分布合作参数，x 分布若为高斯分布，则2β=为最优值。

i 列向量。

（矩阵均方根值可以通过进行柯西因式分解得到）构造x 的样本后，则对应于x 的每个样本点，可以计算对应的y 样本：i ()i y f =ℵ(2)通过y 样本，可以计算,y y P2(m)02(c)0()()Li ii LTy i i i i y W y P W y y y y ==≈≈--∑∑ (3)其中，权重系数i W 可以由下式确定：(m)0()20(m)()11,1,,22()c c i i W L W L W W i LL λλλαβλλ=+=+-++===+ (4)UT 变换不同于蒙特卡洛法，蒙特卡洛法需要产生大量的样本点，而UT 变换不需要。

UT 变换计算非线性函数的统计特性，对于输入为高斯输入的非线性系统，计算精度至少为3阶，对于输入不是高斯输入的非线性系统，计算精度至少可以达到2阶，如果适当选择,αβ 的值，计算精度可以达到3阶或以上。

ukf滤波算法UKF（Unscented Kalman Filter）滤波算法是一种非线性滤波算法，目的是通过逼近非线性系统的状态和测量值的真实分布来估计系统的状态。

相比于传统的Kalman滤波算法，UKF采用了Sigma点来近似系统状态和测量值的分布，从而可以处理非线性系统。

UKF算法的基本思想是使用一些特定的采样点（称为Sigma点）来近似系统状态和测量值的分布。

通过对这些Sigma点进行传播和更新，可以获得系统的状态估计值。

具体来说，UKF算法包含以下几个步骤：1.初始化：确定系统的状态和观测方程，以及状态协方差矩阵和测量噪声协方差矩阵。

2. Sigma点生成：根据系统状态的均值和协方差矩阵，生成一组代表系统状态的Sigma点。

通常，Sigma点的个数是通过经验确定的，一般取2n+1个，其中n是状态向量的维度。

3. Sigma点传播：根据系统的非线性状态方程，通过将Sigma点传播到下一个时刻，得到预测的Sigma点。

这一步骤的目的是在状态空间中对预测状态进行采样。

4.状态预测：利用预测的Sigma点计算出预测的系统状态的均值和协方差矩阵。

5. Sigma点更新：根据测量模型，通过对预测的Sigma点进行线性变换，得到预测的测量值Sigma点。

这一步骤的目的是在测量空间中对预测状态进行采样。

6.测量预测：利用预测的测量值Sigma点计算出预测的测量值的均值和协方差矩阵。

7.卡尔曼增益计算：根据预测的状态和测量值的均值和协方差矩阵，计算出卡尔曼增益。

8.状态更新：利用测量值对预测的状态进行修正，得到更新后的状态估计值和协方差矩阵。

通过以上步骤，UKF算法可以通过对状态和测量值的Sigma点进行传播和更新，逼近非线性系统的状态和测量值的真实分布，从而得到系统的状态估计值。

UKF算法的优点是可以处理非线性系统，并且不需要对系统进行线性化处理。

相比于传统的扩展卡尔曼滤波（EKF）算法，UKF算法更加精确和鲁棒。

基于UKF的滤波算法设计分析与应用共3篇基于UKF的滤波算法设计分析与应用1基于UKF的滤波算法设计分析与应用随着科技的发展，各行各业的数据处理越来越重要，滤波算法在这个过程中扮演了重要的角色。

本文将探讨一种基于UKF的滤波算法，包括其设计分析以及应用。

UKF是一种针对非线性系统的滤波算法，其全称为无迹卡尔曼滤波器（Unscented Kalman Filter）。

相比于卡尔曼滤波器，UKF更加适用于非线性、非高斯的系统，并且其运行速度更快、精度更高。

在UKF的运行过程中，需要进行两次变换，分别为sigma点变换和权值变换，其中sigma点变换将高斯分布的均值和协方差矩阵转换为一些离散的点，这些点在系统的非线性关系下具有良好的近似性质，权值变换则是将这些点的权重求出，最终依据这些点和权重来进行滤波。

在实际应用中，UKF滤波算法及其改进算法大量被应用在各种领域，比如机器人控制、导航、雷达信号处理等等。

本文将以信号处理方面为例，探讨在声音信号处理中，UKF滤波算法的设计分析与应用。

在声音信号处理中，我们常常需要对信号进行滤波以去除噪声，但是传统的滤波算法在处理非线性、非高斯信号时，精度不够高。

因此，UKF滤波算法便成了一个较好的选择。

在设计UKF滤波算法时，需要根据实际需求设置相关参数，比如系统的状态变量和测量变量，以及噪声的协方差矩阵。

在应用过程中，需要将待滤波的信号和上一时刻的状态量带入UKF滤波器进行处理，得到一个经过优化的滤波结果。

在实际应用中，UKF滤波算法在音频降噪方面表现突出，其通过取sigma点进行变换，避免了需要用到高斯假设的问题。

在汽车音响中，UKF滤波算法还可以用于提高音频效果，比如降低回声和噪声，提升车内音质。

此外，在语音识别领域中，UKF滤波算法可以有效提高语音识别的准确性，避免非线性噪声的影响。

需要注意的是，UKF滤波算法并不是万能的，其在处理高维系统时会有一定难度，而且高斯分布的假设仍然是不可取的。

一种基于UKF的天文组合导航滤波算法研究摘要：飞机导航系统的设计需要考虑传感器和外部因素不稳定带来的影响，同时在飞行中也面临着导航系统和量测噪声统计不确定问题，因而导致滤波精度低，稳定性差，有可能发散。

为此本文研究了一种基于UKF的自适应卡尔曼滤波算法，能自动平衡状态信息与观测信息在滤波结果中的权比，来实时调整状态向量和观测向量的协方差，从而提高系统的性能。

仿真结果表明该算法定位精度高，稳定性好，具有重要的工程应用价值。

关键词：UKF 自适应滤波组合导航1、引言SINS能完全独立自主的工作，具有短时精度高、输出连续、抗干扰能力强，可同时提供位置、姿态信息等突出优点，但它误差随时间积累，长时间工作的误差很大；CNS精度高、误差不随时间积累，在所有导航设备中航向精度最高，观测目标为天体不可能被人为摧毁，战争时可用性高，但其输出信息不连续，并且在某些情况下会受到外界环境的影响，如在航空中的应用容易受到气候条件的影响[1]，目前我国比较先进的星敏感器的输出频率为1Hz，对有高精度要求的军用载体来说是不适用的[2]。

由于两者都存在着自身难以克服的缺点，但两者具有互补的特点，所以，将其组合不仅具有独立系统各自的主要优点，而且随着组合水平的加深，它们之间互相交流、使用信息加强，SINS/天文组合系统的总体性能要优于各自独立系统。

本文研究的自适应UKF卡尔曼滤波算法，在系统噪声统计特性未知时，此算法能自动平衡状态信息与观测信息在滤波结果中的权比，来实时调整状态向量和观测向量的协方差，从而提高系统的性能。

2、星敏感器姿态测量误差分析星敏感器是高精度仪器，但也存在多种误差源，主要包括光学系统成像误差，加工、装配误差，光轴不稳定性，CCD噪声、暗电流、性应不均匀性，电子线路噪声，标定误差等。

因此，星敏感器的姿态确定精度实际上受到诸多因素的影响[3]：2.1 星象提取误差星象提取误差主要来源于星光信号本身，包括恒星的自行、光行差、视差、光线弯曲等误差以及星表误差。

无迹卡尔曼滤波指数模型-概述说明以及解释1.引言1.1 概述无迹卡尔曼滤波（UKF）是一种用于非线性系统状态估计的先进滤波算法，通过在高斯分布上取样一些特定点（称为无迹变换），对系统状态的不确定性进行建模。

相比传统的卡尔曼滤波算法，UKF能够更好地处理非线性系统，并且在一些情况下具有更好的性能。

指数模型是一种常见的数学模型，通常用于描述随时间指数增长或减少的趋势。

指数模型在经济学、生物学、工程学等领域中被广泛应用，可以帮助分析和预测未来的趋势变化。

本文将介绍无迹卡尔曼滤波和指数模型的基本原理和应用方法，同时探讨它们在现实生活中的实际应用案例。

通过深入理解这两种模型，我们可以更好地应用它们解决现实世界中的问题，提高系统的状态估计精度和预测准确性。

1.2 文章结构本文主要分为三个部分，即引言、正文和结论。

具体如下：引言部分主要介绍了本文的背景和意义，框定了研究的范围和目的，为读者提供了对整个文章内容的整体认识。

正文部分包括了对无迹卡尔曼滤波和指数模型的介绍，分别讨论了它们的定义、原理和特点，同时给出了相关的数学公式和算法，通过实例分析展示了它们的应用。

结论部分对整篇文章进行了总结，重点强调了无迹卡尔曼滤波和指数模型在实际应用中的价值和意义，并展望了它们的未来发展方向。

同时给出了本文的结论和建议，为读者提供了在相关领域深入研究的参考。

1.3 目的本文旨在探讨无迹卡尔曼滤波和指数模型在数据处理和预测中的应用。

通过分析这两种算法的原理和特点，我们将更深入地了解它们在实际情况下的适用性和效果。

同时，我们也将通过实际案例的介绍来展示这两种算法在不同领域的应用效果，从而为读者提供更多关于无迹卡尔曼滤波和指数模型的了解和应用方法。

我们希望通过本文的阐述，读者能够对这两种算法有一个全面的认识，并在实际工作中灵活运用它们以解决问题和提升工作效率。

2.正文2.1 无迹卡尔曼滤波无迹卡尔曼滤波（UKF）是一种基于传统卡尔曼滤波算法的扩展，旨在解决非线性系统状态估计问题。

UKF的算法介绍——作者，niewei120，nuaa参数说明：其中sigama点的获得方法如下：例外的一种描述方式其滤波算法表示如下：UKF 中各参数的影响：α确定了采样点与均值的远近程度，通常取0 到1 之间的正值，所取值越大则距离平均值越远，当其值为零时，UKF 的滤波效果相当于EKF；UT中的k 为一比例因子，通常取为0，当状态分布可以认为是高斯分布时，可取k = n - 3(n为状态向量x 的维数)；β在正态分布时取2。

当Q 和R 过大时，UKF 的状态非常不稳定，有时甚至还会发散。

整个的matlab的程序如下：（1）sigama点的计算，输出为siagma点和权重值，以及sigama点的数量function [xPts, wPts, nPts] = scaledSymmetricSigmaPoints(x,P,alpha,beta,kappa)% % This function returns the scaled symmetric sigma point distribution.%% [xPts, wPts, nPts] = scaledSymmetricSigmaPoints(x,P,alpha,beta,kappa) %% Inputs:% x mean 状态量均值% P covariance 协方差% alpha scaling parameter 1% a决定万周围Sigma点的分布状态，调节其值能够使得高阶项影响最小% beta extra weight on zero'th point% 调节beta的值可以提高方差的精度，对于高斯分布，beta为2是最优的% kappa scaling parameter 2 (usually set to default 0)% 是一个比例因子kappa=alpha^2*(n+kappa)-n%% Outputs:% xPts The sigma points sigma点% wPts The weights on the points 所在点的权重% nPts The number of points sigma点的数目%%%% (C) 2000 Rudolph van der Merwe% (C) 1998-2000 S. J. Julier.% Number of sigma points and scaling terms sigma点的个数n = size(x(:),1);nPts = 2*n+1; % we're using the symmetric SUT 采用的是对称sui，此时的sigma点为2n+1% Recalculate kappa according to scaling parameters 根据标定参数重新计算kappa值kappa = alpha^2*(n+kappa)-n;% Allocate space 分配空间wPts=zeros(1,nPts);xPts=zeros(n,nPts);% Calculate matrix square root of weighted covariance matrix 计算根号下的协方差矩阵Psqrtm=(chol((n+kappa)*P))';% Array of the sigma points sigma点的阵列xPts=[zeros(size(P,1),1) -Psqrtm Psqrtm];% Add mean back inxPts = xPts + repmat(x,1,nPts);% Array of the weights for each sigma point sigma点的权重阵列wPts=[kappa 0.5*ones(1,nPts-1) 0]/(n+kappa);% Now calculate the zero'th covariance term weight 计算0处的协方差权重wPts(nPts+1) = wPts(1) + (1-alpha^2) + beta;（2）ukf滤波算法function[xEst,PEst,xPred,PPred,zPred,inovation,S,K]=ukf(xEst,PEst,U,Q,ffun,z,R,hfun,dt,alpha,beta,kappa) % TITLE : UNSCENTED KALMAN FILTER%% PURPOSE : This function performs one complete step of the unscented Kalman filter.%% SYNTAX : [xEst,PEst,xPred,PPred,zPred,inovation]=ukf(xEst,PEst,U,Q,ffun,z,R,hfun,dt,alpha,beta,kappa)% % INPUTS : - xEst : state mean estimate at time k k时刻卡尔曼状态量均值的预测值% - PEst : state covariance at time k k时刻协方差的预测值% - U : vector of control inputs 输入的控制量% - Q : process noise covariance at time k k时刻的状态噪声%% - z : observation at k+1 k+1时刻的观测量% - R : measurement noise covariance at k+1 k+1的测量噪声的协方差%% - ffun : process model function 状态方程% - hfun : observation model function 观测方程% - dt : time step (passed to ffun/hfun) 时间步长% - alpha (optional) : sigma point scaling parameter. Defaults to% 1.影响sigma向量围绕分布的扩展因子% - beta (optional) : higher order error scaling parameter.% Default to 0. 另一个规模因子% - kappa (optional) : scalar tuning parameter 1. Defaults to% 0. 先验分布的因子%% OUTPUTS : - xEst : updated estimate of state mean at time% k+1 k+1时刻的状态量的更新值% - PEst : updated state covariance at time k+1% k+1时刻的协方差的更新值% - xPred : prediction of state mean at time k+1% k+1时刻的状态量的预测值% - PPred : prediction of state covariance at time% k+1 k+1时刻的协方差预测值% - inovation : innovation vector 更新矢量% NOTES : The process model is of the form, x(k+1) = ffun[x(k),v(k),dt,u(k)]% where v(k) is the process noise vector. The observation model is% of the form, z(k) = hfun[x(k),w(k),dt,u(k)], where w(k) is the% observation noise vector.%% This code was written to be readable. There is significant% scope for optimisation even in Matlab.%% Process defaultsif (nargin < 10)alpha=1;end;if (nargin < 11)beta=0;end;if (nargin < 12)kappa=0;end;% Calculate the dimensions of the problem and a few useful% scalarsstates = size(xEst(:),1);observations = size(z(:),1);vNoise = size(Q,2); %过程噪声wNoise = size(R,2); %观测噪声noises = vNoise+wNoise;% Augment the state vector with the noise vectors.% Note: For simple, additive noise models this part% can be done differently to save on computational cost.% For details, contact Rudolph v.d. Merweif (noises)N=[Q zeros(vNoise,wNoise); zeros(wNoise,vNoise) R];PQ=[PEst zeros(states,noises);zeros(noises,states) N];xQ=[xEst;zeros(noises,1)];elsePQ=PEst;xQ=xEst;end;% Calculate the sigma points and there corresponding weights using the Scaled Unscented% Transformation[xSigmaPts, wSigmaPts, nsp] = scaledSymmetricSigmaPoints(xQ, PQ, alpha, beta, kappa); %计算sigma点的均值、权重和sigma点的数目% Duplicate wSigmaPts into matrix for code speedup 重复wSigmaPts使得矩阵运算速度提升wSigmaPts_xmat = repmat(wSigmaPts(:,2:nsp),states,1);wSigmaPts_zmat = repmat(wSigmaPts(:,2:nsp),observations,1);% Work out the projected sigma points and their means% This routine is fairly generic. The only thing to watch out for are% angular discontinuities. There is a standard trick for doing this -% contact me (Julier) for details!xPredSigmaPts = feval(ffun,xSigmaPts(1:states,:),repmat(U(:),1,nsp),xSigmaPts(states+1:states+vNoise,:),dt);%状态更新zPredSigmaPts = feval(hfun,xPredSigmaPts,repmat(U(:),1,nsp),xSigmaPts(states+vNoise+1:states+noises,:),dt);%观测更新% Calculate the mean. Based on discussions with C. Schaefer, form% is chosen to maximise numerical robustness.% - I vectorized this part of the code for a speed increase : RvdM 2000xPred = sum(wSigmaPts_xmat .* (xPredSigmaPts(:,2:nsp) - repmat(xPredSigmaPts(:,1),1,nsp-1)),2);%状态量的一步预测值zPred = sum(wSigmaPts_zmat .* (zPredSigmaPts(:,2:nsp) - repmat(zPredSigmaPts(:,1),1,nsp-1)),2);%观测量的一步预测值xPred=xPred+xPredSigmaPts(:,1);zPred=zPred+zPredSigmaPts(:,1);% Work out the covariances and the cross correlations. Note that% the weight on the 0th point is different from the mean% calculation due to the scaled unscented algorithm.exSigmaPt = xPredSigmaPts(:,1)-xPred; %sigma点状态值的误差ezSigmaPt = zPredSigmaPts(:,1)-zPred; %sigma点量测值的误差PPred = wSigmaPts(nsp+1)*exSigmaPt*exSigmaPt';%状态量的协方差PxzPred = wSigmaPts(nsp+1)*exSigmaPt*ezSigmaPt';%状态量与观测量的协方差S = wSigmaPts(nsp+1)*ezSigmaPt*ezSigmaPt';%观测量的协方差exSigmaPt = xPredSigmaPts(:,2:nsp) - repmat(xPred,1,nsp-1);%sigma点状态值的误差的更新ezSigmaPt = zPredSigmaPts(:,2:nsp) - repmat(zPred,1,nsp-1);%sigma点量测值的误差的更新PPred = PPred + (wSigmaPts_xmat .* exSigmaPt) * exSigmaPt';%状态量的协方差的更新S = S + (wSigmaPts_zmat .* ezSigmaPt) * ezSigmaPt';%观测量的协方差的更新PxzPred = PxzPred + exSigmaPt * (wSigmaPts_zmat .* ezSigmaPt)';%状态量与观测量的协方差的更新%%%%% MEASUREMENT UPDATE 量测更新% Calculate Kalman gain 计算卡尔曼增益K = PxzPred / S;% Calculate Innovation 计算测量误差inovation = z - zPred;% Update mean 更新状态量均值xEst = xPred + K*inovation;% Update covariance 更新协方差PEst = PPred - K*S*K';。

无迹卡尔曼滤波算法
无迹卡尔曼滤波算法（Unscented Kalman Filter，UKF）是一种用于处理非线性系统的非参数滤波算法，它可以从观测和测量数据中推断隐藏状态的值。

UKF的基本思想是基于状态变量的状态和测量变量的观测，使用一系列加权的状态估计来预测未来状态，并通过观测和测量值来校正预测值。

UKF的优点在于它可以处理非线性系统，而不需要对系统进行线性化处理，从而可以更准确地估计隐藏状态变量，准确度比传统卡尔曼滤波算法更高。

UKF是一种经典的非线性滤波算法，它可以利用观测和测量值，以及相关的不确定性信息，以准确的方式估计隐藏状态变量。

它也可以用于自适应控制，机器人移动控制，机器视觉，自动驾驶等领域。

UKF可以用来模拟复杂的物理过程，估计不同的系统参数，以及更准确地预测未来的状态，这在许多领域，如自动驾驶汽车，智能机器人，机器视觉，航空航天，大气科学和精细化工等领域中都很有用。

总之，无迹卡尔曼滤波算法是一种用于处理非线性系统的有效滤波算法，能够从观测和测量数据中推断隐藏状态的值，准确度比传统卡尔曼滤波算法更高，在航空航天，机器人，机器视觉，控制系统，大气科学和精细化工等领域都得到了广泛应用。