在线状态估计的扩展和Uncented Kalman滤波器算法 - Matlab＆Simulink万博1manbetx

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

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

在命令行处，您使用该函数来构造扩展卡尔曼滤波器或非中心过滤器对象，并指定函数中的过程和测量噪声项是加性的还是非加性的。创建对象后，使用预测和正确的命令用实时数据估计各种状态。有关执行这些命令的顺序的信息，请参阅预测和正确的参考页面。

在Si万博1manbetxmulink中，您可以在扩展卡尔曼滤波器和Unscented Kalman Filter.阻碍。还可以指定函数中的过程噪声项和测量噪声项是相加的还是非相加的。在这些块中，软件决定进行状态估计的预测和校正的顺序。

扩展卡尔曼滤波算法

这个扩展卡尔曼滤波器命令和扩展卡尔曼滤波器块实现一阶离散时间卡尔曼滤波器算法。假设用于离散时间非线性系统的状态转换和测量方程具有非加法过程和具有零均值和协方差矩阵的测量噪声术语Q和R，分别为：

$\begin{array}{l} X [K + 1.] = F (X [K], W [K], U_{s} [K]) \\ y [K] = H (X [K], v [K], U_{M} [K]) \\ W [K] 〜 (0, Q [K]) \\ v [K] 〜 (0, R [K]) \end{array}$

在这里F是描述状态演化的非线性状态转移函数吗X从一个时间步到下一个时间步。非线性测量函数H有关X测量y在时间步K．这些函数还可以具有所用的其他输入参数U_s和U_M. 对过程噪声和测量噪声进行了分析W和v,分别。您提供Q和R．

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

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

$\begin{array}{l} \hat{X} [0 | - 1.] = E (X [0]) \\ P [0 | - 1.] = E (X [0] - \hat{X} [0 | - 1.]) (^{X [0] - \hat{X} [0 | - 1.]) T} \end{array}$

在这里 $\hat{X}$ 是国家的估计和 $\hat{X} [K_{A.} | K_{B}]$ 为时间步长的状态估计K_A.在时间步骤使用测量0,1，...，k_B．所以 $\hat{X} [0 | - 1.]$ 是进行任何测量之前状态值的最佳猜测。在构造过滤器时指定此值。
时间步骤K= 0,1,2,3，...，执行以下操作：
1. 计算测量函数的Jacobian，并使用测量数据更新状态和状态估计错误协方差，y [k]。在命令行中正确的命令执行此更新。
  
  $\begin{array}{l} C [K] = {\frac{\partial H}{\partial X} |}_{\hat{X} [K | K - 1.]} \\ s [K] = {\frac{\partial H}{\partial v} |}_{\hat{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} + s [K] R [K] s [^{K] T}) - 1.} \\ \hat{X} [K | K] = \hat{X} [K | K - 1.] + K [K] (y [K] - H (\hat{X} [K | K - 1.], 0, U_{M} [K]) \\ \hat{P} [K | K] = P [K | K - 1.] - K [K] C [K] P [K | K - 1.] \end{array}$
  
  在这里K就是卡尔曼增益。
2. 计算状态转换函数的Jacobian，并在下次步骤中预测状态和状态估计错误协方差。在软件中，预测命令执行此预测。
  
  $\begin{array}{l} A. [K] = {\frac{\partial F}{\partial X} |}_{\hat{X} [K | K]} \\ G [K] = {\frac{\partial F}{\partial W} |}_{\hat{X} [K | K]} \end{array}$
  
  除非指定分析雅可比矩阵，否则软件将以数值方式计算这些雅可比矩阵。这种数值计算可能会增加处理时间和状态估计的数值精度。
  
  $\begin{array}{l} P [K + 1. | K] = A. [K] P [K | K] A. [^{K] T} + G [K] Q [K] G [^{K] T} \\ \hat{X} [K + 1. | K] = F (\hat{X} [K | K], 0, U_{s} [K]) \end{array}$
  
  这个正确的函数在下一时间步中使用这些值。为了获得更好的数值性能，软件使用协方差矩阵的平方根分解。有关此因式分解的更多信息，请参阅[2]．

这个扩展卡尔曼滤波器块支持多个测万博1manbetx量功能。只要其采样时间是状态转换采样时间的整数倍数，这些测量值可以具有不同的采样时间。在这种情况下，对应于来自每个测量功能的测量来执行单独的校正步骤。

先前描述的算法步骤假设在状态转换和测量功能中具有非加性噪声术语。如果您在功能中具有添加性噪声术语，则算法中的更改是：

如果过程噪音W是加法，那是状态转移方程的形式吗 $X [K] = F (X [K - 1.], U_{s} [K - 1.]) + W [K - 1.]$ ，然后是雅各比亚矩阵G [k]是一个单位矩阵。
如果测量噪声v是添加剂，即测量方程具有表单 $y [K] = H (X [K], U_{M} [K]) + v [K]$ ，然后是雅各比亚矩阵S [k]是一个单位矩阵。

状态函数和过渡函数中的附加噪声项缩短了处理时间。

一阶扩展卡尔曼滤波器对非线性状态转移和测量函数使用线性近似。因此，如果系统中存在严重的非线性，则该算法可能不可靠。在这种情况下，unscented卡尔曼滤波算法可能会产生更好的结果。

Unscented Kalman滤波算法

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

假设状态转换和测量方程M-状态离散非线性系统具有零均值和零协方差的加性过程项和测量噪声项Q和R，分别为：

$\begin{array}{l} X [K + 1.] = F (X [K], U_{s} [K]) + W [K] \\ y [K] = H (X [K], U_{M} [K]) + v [K] \\ W [K] 〜 (0, Q [K]) \\ v [K] 〜 (0, R [K]) \end{array}$

您可以提供Q和R在ProcessNoise和MeasurementNoiseUnscented Kalman Filter对象的属性。

使用状态的初始值初始化过滤器对象，x[0]，以及状态估计误差协方差，P．

$\begin{array}{l} \hat{X} [0 | - 1.] = E (X [0]) \\ P [0 | - 1.] = E (X [0] - \hat{X} [0 | - 1.]) (^{X [0] - \hat{X} [0 | - 1.]) T} \end{array}$

在这里 $\hat{X}$ 是国家的估计和 $\hat{X} [K_{A.} | K_{B}]$ 为时间步长的状态估计K_A.在时间步骤使用测量0,1，...，k_B．所以 $\hat{X} [0 | - 1.]$ 是进行任何测量之前状态值的最佳猜测。在构造过滤器时指定此值。
对于每个时间步骤K，利用实测数据更新状态和状态估计误差协方差，y [k]. 在软件中正确的命令执行此更新。
1. 选择西格玛点 ${\hat{X}}^{(我)} [K | K - 1.]$ 在时间步K．
  
  $\begin{array}{l} {\hat{X}}^{(0)} [K | K - 1.] = \hat{X} [K | K - 1.] \\ {\hat{X}}^{(我)} [K | K - 1.] = \hat{X} [K | K - 1.] + Δ X^{(我)} 我 = 1., ..., 2. M \\ Δ X^{(我)} = (\sqrt{C P [K | K - 1.]}) {}_{我} 我 = 1., ..., M \\ Δ X^{(M + 我)} = - {(\sqrt{C P [K | K - 1.]})}_{我} 我 = 1., ..., M \end{array}$
  
  哪里 $C = α^{2.} (M + κ)$ 是一个基于状态数M的比例因子，还有参数α和κ．有关参数的更多信息，请参见Alpha，Beta和Kappa参数的影响． $\sqrt{C P}$ 这个矩阵是的平方根吗内容提供商这样 $\sqrt{C P} {(\sqrt{C P})}^{T} = C P$ 和 ${(\sqrt{C P})}_{我}$ 是我柱子 $\sqrt{C P}$ ．
2. 使用非线性测量功能来计算每个Sigma点的预测测量值。
  
  ${\hat{y}}^{(我)} [K | K - 1.] = H ({\hat{X}}^{(我)} [K | K - 1.], U_{M} [K]）我 = 0, 1., ..., 2. M$
3. 结合预测的测量值以获得预测测量K．
  
  $\begin{array}{l} \hat{y} [K] = \sum_{我 = 0}^{2. M} W_{M}^{(我)} {\hat{y}}^{(我)} [K | K - 1.] \\ W_{M}^{(0)} = 1. - \frac{M}{α^{2.} (M + κ)} \\ W_{M}^{我} = \frac{1.}{2. α^{2.} (M + κ)} 我 = 1., 2., ..., 2. M \end{array}$
4. 估计预测测量的协方差。添加R [k]来解释附加的测量噪声。
  
  $\begin{array}{l} P_{y} = \sum_{我 = 0}^{2. M} W_{C}^{(我)} ({\hat{y}}^{(我)} [K | K - 1.] - \hat{y} [K]) (^{{\hat{y}}^{(我)} [K | K - 1.] - \hat{y} [K]) T} + R [K] \\ W_{C}^{(0)} = (2. - α^{2.} + β) - \frac{M}{α^{2.} (M + κ)} \\ W_{C}^{我} = 1. / (2. α^{2.} (M + κ)) 我 = 1., 2., ..., 2. M \end{array}$
  
  和有关信息。β参数，参见Alpha，Beta和Kappa参数的影响．
5. 估计之间的互协方差 $\hat{X} [K | K - 1.]$ 和 $\hat{y} [K]$ ．
  
  $P_{X y} = \frac{1.}{2. α^{2.} (M + κ)} \sum_{我 = 1.}^{2. M} ({\hat{X}}^{(我)} [K | K - 1.] - \hat{X} [K | K - 1.]) {({\hat{y}}^{(我)} [K | K - 1.] - \hat{y} [K])}^{T}$
  
  总和从我= 1,因为 ${\hat{X}}^{(0)} [K | K - 1.] - \hat{X} [K | K - 1.] = 0$ ．
6. 得到时间步长的状态估计和状态估计误差协方差K．
  
  $\begin{array}{l} K = P_{X y} P_{y}^{- 1.} \\ \hat{X} [K | K] = \hat{X} [K | K - 1.] + K (y [K] - \hat{y} [K]) \\ P [K | K] = P [K | K - 1.] - K P_{y} K_{K}^{T} \end{array}$
  
  在这里K就是卡尔曼增益。
在下一时间步预测状态和状态估计误差协方差。在软件中预测命令执行此预测。
1. 选择西格玛点 ${\hat{X}}^{(我)} [K | K]$ 在时间步K．
  
  $\begin{array}{l} {\hat{X}}^{(0)} [K | K] = \hat{X} [K | K] \\ {\hat{X}}^{(我)} [K | K] = \hat{X} [K | K] + Δ X^{(我)} 我 = 1., ..., 2. M \\ Δ X^{(我)} = (\sqrt{C P [K | K]}) {}_{我} 我 = 1., ..., M \\ Δ X^{(M + 我)} = - {(\sqrt{C P [K | K]})}_{我} 我 = 1., ..., M \end{array}$
2. 使用非线性状态转移函数来计算每个西格玛点的预测状态。
  
  ${\hat{X}}^{(我)} [K + 1. | K] = F ({\hat{X}}^{(我)} [K | K], U_{s} [K]）$
3. 组合预测状态，得到时刻的预测状态K + 1．控件使用这些值正确的命令，在下一个时间步骤中执行。
  
  $\begin{array}{l} \hat{X} [K + 1. | K] = \sum_{我 = 0}^{2. M} W_{M}^{(我)} {\hat{X}}^{(我)} [K + 1. | K] \\ W_{M}^{(0)} = 1. - \frac{M}{α^{2.} (M + κ)} \\ W_{M}^{我} = \frac{1.}{2. α^{2.} (M + κ)} 我 = 1., 2., ..., 2. M \end{array}$
4. 计算预测状态的协方差。添加q [k]考虑加性过程噪声。这些值由正确的命令，在下一个时间步骤中执行。
  
  $\begin{array}{l} P [K + 1. | K] = \sum_{我 = 0}^{2. M} W_{C}^{(我)} ({\hat{X}}^{(我)} [K + 1. | K] - \hat{X} [K + 1. | K]) {({\hat{X}}^{(我)} [K + 1. | K] - \hat{X} [K + 1. | K])}^{T} + Q [K] \\ W_{C}^{(0)} = (2. - α^{2.} + β) - \frac{M}{α^{2.} (M + κ)} \\ W_{C}^{我} = 1. / (2. α^{2.} (M + κ)) 我 = 1., 2., ..., 2. M \end{array}$

这个Unscented Kalman Filter.块支持多个测万博1manbetx量功能。只要其采样时间是状态转换采样时间的整数倍数，这些测量值可以具有不同的采样时间。在这种情况下，对应于来自每个测量功能的测量来执行单独的校正步骤。

假设状态转换和测量方程中的添加性噪声术语实现了先前的算法。为了更好的数值性能，软件使用协方差矩阵的平方根分解。有关此分组的更多信息，请参阅[2]．

如果噪声术语是非添加剂，则算法的主要变化是：

这个正确的命令生成2 *（m + v）+1Sigma点使用p [k | k-1]和R [k]，在哪里v是测量噪声中的元素数v [k]．这个R [k]在算法步骤2（d）中不再添加术语，因为额外的西格玛点捕获了测量噪声对测量结果的影响派克．
这个预测命令生成2 *（m + w）+1Sigma点使用p [k | k]和q [k]，在哪里W工艺噪声中元素的数量是多少w [k]．这个q [k]算法步骤3（d）中不再添加术语，因为额外的sigma点捕获过程噪声的影响p [k + 1 | k]．

Alpha，Beta和Kappa参数的影响

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

围绕平均状态值的Sigma点的扩展由两个参数控制α和κ．第三个参数，β，在状态和测量协方差计算期间影响变换点的权重。

α-确定平均状态值周围西格玛点的分布。它通常是一个小的正值。西格玛点的扩展与α．较小的值对应于更接近均值状态的点。
κ-第二个缩放参数，通常设置为0。较小的值对应于更接近均值状态的点。扩散与的平方根成正比κ．
β- 纳入国家分布的先验知识。对于高斯分布，β= 2是最佳的。

可以在中指定这些参数Α,卡巴,贝塔无味卡尔曼滤波器的特性。如果知道状态和状态协方差的分布，可以调整这些参数以捕获分布的高阶矩的变换。该算法只能跟踪状态概率分布中的单峰。如果系统的状态分布中有多个峰值，则可以调整这些参数，使sigma点保持在单个峰值附近。例如，选择一个小的Α生成接近平均状态值的Sigma点。

参考文献

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

[2] van der Merwe，Rudolph和Eric A. Wan。“Square-Root Uncented Kalman滤波器用于状态和参数估计。”2001 IEEE声学，语音和信号处理国际会议。议程(猫。No.01CH37221), 6:3461 - 64。盐湖城，德克萨斯，美国:IEEE, 2001。https://doi.org/10.1109/ICASSP.2001.940586。