在线状态估计的扩展和无迹卡尔曼滤波算法- MATLAB和Simulink金宝app

在线状态估计的扩展和无迹卡尔曼滤波算法

可以使用离散时间扩展卡尔曼滤波算法和无迹卡尔曼滤波算法对离散时间非线性系统进行在线状态估计。如果你有一个严重非线性的系统，无迹卡尔曼滤波算法可能会给出更好的估计结果。您可以在Simulink中执行状态估计金宝app^®在命令行。要执行状态估计，首先要为系统创建非线性状态转移函数和测量函数。

在命令行中，使用函数来构造extendedKalmanFilter或unscentedKalmanFilter对象，并指定函数中的过程和测量噪声项是加性的还是非加性的。创建对象后，使用预测和正确的使用实时数据估计状态的命令。有关执行这些命令的顺序的信息，请参见预测和正确的参考页面。

在Si金宝appmulink中，您可以在扩展卡尔曼滤波器和无味卡尔曼滤波块。您还可以指定函数中的过程和测量噪声项是可加性的还是非可加性的。在块中，软件决定状态估计的预测和校正的顺序。

扩展卡尔曼滤波算法

的extendedKalmanFilter命令和扩展卡尔曼滤波器块实现了一阶离散时间卡尔曼滤波算法。假设离散时间非线性系统的状态转移和测量方程具有非加性过程和测量噪声项，且均为零均值和协方差矩阵问和R分别为:

$\begin{array}{l} x ［ k + 1 ］＝ f （ x ［ k ］， w ［ k ］， u_{年代} ［ k ］） \\ y ［ k ］＝ h （ x ［ k ］， v ［ k ］， u_{米} ［ k ］） \\ w ［ k ］～（ 0 ，问［ k ］） \\ v ［ k ］～（ 0 ， R ［ k ］） \end{array}$

在这里f是描述状态演化的非线性状态转移函数吗x从一个时间步骤到下一个时间步骤。非线性测量函数h有关x的测量y在时间步k．这些函数还可以有附加的输入参数u_年代和u_米．过程和测量噪声w和v,分别。您提供问和R．

在该模块中，软件决定状态估计的预测和修正顺序。在命令行中，您可以决定顺序。有关执行这些命令的顺序的信息，请参见预测和正确的参考页面。假设你实现了正确的命令之前预测，软件实现算法如下:

用状态的初始值初始化筛选器对象，x [0]，状态估计误差协方差矩阵，P．

$\begin{array}{l} \overset{＾}{x} ［ 0 | - 1 ］＝ E （ x ［ 0 ］） \\ P ［ 0 | - 1 ］＝ E （ x ［ 0 ］ - \overset{＾}{x} ［ 0 | - 1 ］）（^{x ［ 0 ］ - \overset{＾}{x} ［ 0 | - 1 ］） T} \end{array}$

在这里 $\overset{＾}{x}$ 是州估计和 $\overset{＾}{x} ［ k_{一个} | k_{b} ］$ 为时间步长的状态估计k_一个使用时间步长的测量0,1,…,k_b．所以 $\overset{＾}{x} ［ 0 | - 1 ］$ 是在进行任何测量之前对状态值的最佳猜测。在构造过滤器时指定此值。
时间的步骤k= 0, 1, 2, 3,……，perform the following:
1. 计算测量函数的雅可比矩阵，利用测量数据更新状态和状态估计误差协方差，y [k]．在命令行中，正确的命令执行此更新。
  
  $\begin{array}{l} C ［ k ］＝ {\frac{\partial h}{\partial x} |}_{\overset{＾}{x} ［ k | k - 1 ］} \\ 年代［ k ］＝ {\frac{\partial h}{\partial v} |}_{\overset{＾}{x} ［ k | k - 1 ］} \end{array}$
  
  软件计算这些雅可比矩阵数值除非你指定解析雅可比矩阵。
  
  $\begin{array}{l} K ［ k ］＝ P ［ k | k - 1 ］ C ［^{k ］ T} （^{C ［ k ］ P ［ k | k - 1 ］ C ［^{k ］ T} + 年代［ k ］ R ［ k ］年代［^{k ］ T} ） - 1} \\ \overset{＾}{x} ［ k | k ］＝ \overset{＾}{x} ［ k | k - 1 ］ + K ［ k ］（ y ［ k ］ - h （ \overset{＾}{x} ［ k | k - 1 ］， 0 ， u_{米} ［ k ］） \\ \overset{＾}{P} ［ k | k ］＝ P ［ k | k - 1 ］ - K ［ k ］ C ［ k ］ P ［ k | k - 1 ］ \end{array}$
  
  在这里K就是卡尔曼增益。
2. 计算状态转移函数的雅可比矩阵，并预测下一时刻的状态和状态估计误差协方差。在软件中预测命令执行此预测。
  
  $\begin{array}{l} 一个［ k ］＝ {\frac{\partial f}{\partial x} |}_{\overset{＾}{x} ［ k | k ］} \\ G ［ k ］＝ {\frac{\partial f}{\partial w} |}_{\overset{＾}{x} ［ k | k ］} \end{array}$
  
  软件计算这些雅可比矩阵数值除非你指定解析雅可比矩阵。这种数值计算可能会增加状态估计的处理时间和数值误差。
  
  $\begin{array}{l} P ［ k + 1 | k ］＝一个［ k ］ P ［ k | k ］一个［^{k ］ T} + G ［ k ］问［ k ］ G ［^{k ］ T} \\ \overset{＾}{x} ［ k + 1 | k ］＝ f （ \overset{＾}{x} ［ k | k ］， 0 ， u_{年代} ［ k ］） \end{array}$
  
  的正确的函数在下一个时间步骤中使用这些值。为了更好的数值性能，该软件使用协方差矩阵的平方根分解。有关此分解的更多信息，请参见［２］．

的扩展卡尔曼滤波器块支持多种测金宝app量功能。这些测量可以有不同的采样时间，只要它们的采样时间是状态转移采样时间的整数倍。在这种情况下，针对每个测量函数的测量值执行单独的校正步骤。

前面描述的算法步骤假设在状态转换和测量函数中有非加性噪声项。如果函数中有加性噪声项，算法的变化是:

如果过程噪声w是加法，那是状态转移方程的形式吗 $x ［ k ］＝ f （ x ［ k - 1 ］， u_{年代} ［ k - 1 ］） + w ［ k - 1 ］$ ，然后是雅可比矩阵G [k]是单位矩阵。
如果测量噪声v加法，就是测量方程的形式 $y ［ k ］＝ h （ x ［ k ］， u_{米} ［ k ］） + v ［ k ］$ ，然后是雅可比矩阵S [k]是单位矩阵。

状态函数和过渡函数中的加性噪声项减少了处理时间。

一阶扩展卡尔曼滤波器对非线性状态转移和测量函数采用线性逼近。因此，如果系统中的非线性很严重，算法可能就不可靠。在这种情况下，采用无迹卡尔曼滤波算法可以得到较好的结果。

无迹卡尔曼滤波算法

无迹卡尔曼滤波算法无味卡尔曼滤波块使用无气味变换捕捉传播的状态估计的统计性质通过非线性函数。该算法首先生成一组称为sigma点的状态值。这些点捕获了状态估计的均值和协方差。该算法使用每个sigma点作为状态转移和测量函数的输入，以获得一组新的转换状态点。然后利用变换点的均值和协方差得到状态估计和状态估计误差协方差。

假设某型的状态跃迁和测量方程米状态离散非线性系统具有具有零均值和零协方差的可加过程和测量噪声项问和R分别为:

$\begin{array}{l} x ［ k + 1 ］＝ f （ x ［ k ］， u_{年代} ［ k ］） + w ［ k ］ \\ y ［ k ］＝ h （ x ［ k ］， u_{米} ［ k ］） + v ［ k ］ \\ w ［ k ］～（ 0 ，问［ k ］） \\ v ［ k ］～（ 0 ， R ［ k ］） \end{array}$

提供的初始值问和R在ProcessNoise和MeasurementNoise无迹卡尔曼滤波对象的性质。

用状态的初始值初始化筛选器对象，x [0]，状态估计误差协方差，P．

$\begin{array}{l} \overset{＾}{x} ［ 0 | - 1 ］＝ E （ x ［ 0 ］） \\ P ［ 0 | - 1 ］＝ E （ x ［ 0 ］ - \overset{＾}{x} ［ 0 | - 1 ］）（^{x ［ 0 ］ - \overset{＾}{x} ［ 0 | - 1 ］） T} \end{array}$

在这里 $\overset{＾}{x}$ 是州估计和 $\overset{＾}{x} ［ k_{一个} | k_{b} ］$ 为时间步长的状态估计k_一个使用时间步长的测量0,1,…,k_b．所以 $\overset{＾}{x} ［ 0 | - 1 ］$ 是在进行任何测量之前对状态值的最佳猜测。在构造过滤器时指定此值。
对于每个时间步骤k，利用实测数据更新状态和状态估计误差协方差，y [k]．在软件中正确的命令执行此更新。
1. 选择点 ${\overset{＾}{x}}^{（我）} ［ k | k - 1 ］$ 在时间步k．
  
  $\begin{array}{l} {\overset{＾}{x}}^{（ 0 ）} ［ k | k - 1 ］＝ \overset{＾}{x} ［ k | k - 1 ］ \\ {\overset{＾}{x}}^{（我）} ［ k | k - 1 ］＝ \overset{＾}{x} ［ k | k - 1 ］ + Δ x^{（我）} 我＝ 1 ，．.. ， 2 米 \\ Δ x^{（我）} ＝（ \sqrt{c P ［ k | k - 1 ］} ） {}_{我} 我＝ 1 ，．.. ，米 \\ Δ x^{（米 + 我）} ＝ - {（ \sqrt{c P ［ k | k - 1 ］} ）}_{我} 我＝ 1 ，．.. ，米 \end{array}$
  
  在哪里 $c ＝ α^{2} （米 + κ ）$ 是一个基于状态数M的比例因子，还有参数α和κ．有关参数的更多信息，请参见Alpha, Beta和Kappa参数的影响． $\sqrt{c P}$ 这个矩阵是的平方根吗cP这样 $\sqrt{c P} {（ \sqrt{c P} ）}^{T} ＝ c P$ 和 ${（ \sqrt{c P} ）}_{我}$ 是我th列 $\sqrt{c P}$ ．
2. 使用非线性测量函数来计算每个西格玛点的预测测量值。
  
  ${\overset{＾}{y}}^{（我）} ［ k | k - 1 ］＝ h （ {\overset{＾}{x}}^{（我）} ［ k | k - 1 ］， u_{米} [k]) 我＝ 0 ， 1 ，．.. ， 2 米$
3. 将预测的测量值组合起来，得到时刻的预测测量值k．
  
  $\begin{array}{l} \overset{＾}{y} ［ k ］＝ \sum_{我＝ 0}^{2 米} W_{米}^{（我）} {\overset{＾}{y}}^{（我）} ［ k | k - 1 ］ \\ W_{米}^{（ 0 ）} ＝ 1 - \frac{米}{α^{2} （米 + κ ）} \\ W_{米}^{我} ＝ \frac{1}{2 α^{2} （米 + κ ）} 我＝ 1 ， 2 ，．.. ， 2 米 \end{array}$
4. 估计预测测量的协方差。添加R [k]来解释附加的测量噪声。
  
  $\begin{array}{l} P_{y} ＝ \sum_{我＝ 0}^{2 米} W_{c}^{（我）} （ {\overset{＾}{y}}^{（我）} ［ k | k - 1 ］ - \overset{＾}{y} ［ k ］）（^{{\overset{＾}{y}}^{（我）} ［ k | k - 1 ］ - \overset{＾}{y} ［ k ］） T} + R ［ k ］ \\ W_{c}^{（ 0 ）} ＝（ 2 - α^{2} + β ） - \frac{米}{α^{2} （米 + κ ）} \\ W_{c}^{我} ＝ 1 / （ 2 α^{2} （米 + κ ））我＝ 1 ， 2 ，．.. ， 2 米 \end{array}$
  
  和有关信息。β参数,看到Alpha, Beta和Kappa参数的影响．
5. 估计之间的交叉协方差 $\overset{＾}{x} ［ k | k - 1 ］$ 和 $\overset{＾}{y} ［ k ］$ ．
  
  $P_{x y} ＝ \frac{1}{2 α^{2} （米 + κ ）} \sum_{我＝ 1}^{2 米} （ {\overset{＾}{x}}^{（我）} ［ k | k - 1 ］ - \overset{＾}{x} ［ k | k - 1 ］） {（ {\overset{＾}{y}}^{（我）} ［ k | k - 1 ］ - \overset{＾}{y} ［ k ］）}^{T}$
  
  求和从我= 1,因为 ${\overset{＾}{x}}^{（ 0 ）} ［ k | k - 1 ］ - \overset{＾}{x} ［ k | k - 1 ］＝ 0$ ．
6. 得到时间步长的状态估计和状态估计误差协方差k．
  
  $\begin{array}{l} K ＝ P_{x y} P_{y}^{- 1} \\ \overset{＾}{x} ［ k | k ］＝ \overset{＾}{x} ［ k | k - 1 ］ + K （ y ［ k ］ - \overset{＾}{y} ［ k ］） \\ P ［ k | k ］＝ P ［ k | k - 1 ］ - K P_{y} K_{k}^{T} \end{array}$
  
  在这里K就是卡尔曼增益。
预测下一个时间步骤的状态和状态估计误差协方差。在软件中预测命令执行此预测。
1. 选择点 ${\overset{＾}{x}}^{（我）} ［ k | k ］$ 在时间步k．
  
  $\begin{array}{l} {\overset{＾}{x}}^{（ 0 ）} ［ k | k ］＝ \overset{＾}{x} ［ k | k ］ \\ {\overset{＾}{x}}^{（我）} ［ k | k ］＝ \overset{＾}{x} ［ k | k ］ + Δ x^{（我）} 我＝ 1 ，．.. ， 2 米 \\ Δ x^{（我）} ＝（ \sqrt{c P ［ k | k ］} ） {}_{我} 我＝ 1 ，．.. ，米 \\ Δ x^{（米 + 我）} ＝ - {（ \sqrt{c P ［ k | k ］} ）}_{我} 我＝ 1 ，．.. ，米 \end{array}$
2. 使用非线性状态转移函数来计算每个西格玛点的预测状态。
  
  ${\overset{＾}{x}}^{（我）} ［ k + 1 | k ］＝ f （ {\overset{＾}{x}}^{（我）} ［ k | k ］， u_{年代} [k])$
3. 组合预测状态，得到时刻的预测状态k + 1．控件使用这些值正确的命令，在下一个时间步骤中执行。
  
  $\begin{array}{l} \overset{＾}{x} ［ k + 1 | k ］＝ \sum_{我＝ 0}^{2 米} W_{米}^{（我）} {\overset{＾}{x}}^{（我）} ［ k + 1 | k ］ \\ W_{米}^{（ 0 ）} ＝ 1 - \frac{米}{α^{2} （米 + κ ）} \\ W_{米}^{我} ＝ \frac{1}{2 α^{2} （米 + κ ）} 我＝ 1 ， 2 ，．.. ， 2 米 \end{array}$
4. 计算预测状态的协方差。添加问[k]考虑到添加的过程噪声。控件使用这些值正确的命令，在下一个时间步骤中执行。
  
  $\begin{array}{l} P ［ k + 1 | k ］＝ \sum_{我＝ 0}^{2 米} W_{c}^{（我）} （ {\overset{＾}{x}}^{（我）} ［ k + 1 | k ］ - \overset{＾}{x} ［ k + 1 | k ］） {（ {\overset{＾}{x}}^{（我）} ［ k + 1 | k ］ - \overset{＾}{x} ［ k + 1 | k ］）}^{T} + 问［ k ］ \\ W_{c}^{（ 0 ）} ＝（ 2 - α^{2} + β ） - \frac{米}{α^{2} （米 + κ ）} \\ W_{c}^{我} ＝ 1 / （ 2 α^{2} （米 + κ ））我＝ 1 ， 2 ，．.. ， 2 米 \end{array}$

的无味卡尔曼滤波块支持多种测金宝app量功能。这些测量可以有不同的采样时间，只要它们的采样时间是状态转移采样时间的整数倍。在这种情况下，针对每个测量函数的测量值执行单独的校正步骤。

前面的算法是在状态转移和测量方程中假定加性噪声项的情况下实现的。为了更好的数值性能，该软件使用协方差矩阵的平方根分解。有关此分解的更多信息，请参见［２］．

如果噪声项是非加性的，算法的主要变化是:

的正确的命令生成2 * (M + V) + 1σ点使用P (k | k - 1)和R [k],在那里V测量的元素个数是否有噪声v [k]．的R [k]项不再添加到算法步骤2(d)中，因为额外的sigma点捕获了测量噪声的影响Py．
的预测命令生成2 * (M + W) + 1σ点使用P (k | k)和问[k],在那里W工艺噪声中元素的数量是多少w [k]．的问[k]项不再添加到算法步骤3(d)中，因为额外的sigma点捕获了过程噪声的影响P (k + 1 | k)．

Alpha, Beta和Kappa参数的影响

为了计算下一时刻的状态及其统计特性，无迹卡尔曼滤波算法生成一组分布在状态均值周围的状态值。该算法使用每个sigma点作为状态转移和测量函数的输入，以获得一组新的转换状态点。然后利用变换点的均值和协方差得到状态估计和状态估计误差协方差。

σ点在状态均值周围的分布由两个参数控制α和κ．第三个参数,β，在状态和度量协方差计算期间影响转换点的权重。

α-决定状态均值周围sigma点的分布。它通常是一个小的正值。点的分布是成比例的α．较小的值对应于更接近均值状态的点。
κ-第二个缩放参数，通常设置为0。较小的值对应于更接近均值状态的点。扩散与的平方根成正比κ．
β-整合了州分布的先验知识。为高斯分布,β= 2是最优的。

属性中指定这些参数α，卡巴,β无迹卡尔曼滤波器的性质。如果你知道状态和状态协方差的分布，你可以调整这些参数来捕捉分布的高阶矩的变换。该算法只能跟踪状态概率分布中的单个峰值。如果在你的系统的状态分布中有多个峰值，你可以调整这些参数，使σ点停留在一个峰值附近。例如，选择一个小的α生成接近状态均值的点。

参考文献

[1]西门,丹。最优状态估计:卡尔曼、H∞和非线性方法．霍博肯:约翰·威利父子公司，2006年。

范·德·默维，鲁道夫和埃里克·温。状态和参数估计的平方根无迹卡尔曼滤波器2001年IEEE声学、语音和信号处理国际会议。诉讼(猫。No.01CH37221), 6:3461 - 64。盐湖城，德克萨斯，美国:IEEE, 2001。https://doi.org/10.1109/ICASSP.2001.940586。