无迹卡尔曼滤波与非线性估计（UKF）论文及具体实现讲解

无迹卡尔曼滤波与非线性估计

论文作者：Simon J. Julier, Jeffrey K. Uhlmann

摘要

扩展卡尔曼滤波(EKF)可能是应用最广泛的非线性系统估计算法。然而,估计界超过35年的经验表明,EKF难以实现,难以调试,只对更新时间尺度上近似线性的系统可靠。这些困难大多源于它使用了线性化。为克服这一局限性,提出了无迹变换(Unscented Transform, UT)作为一种通过非线性变换传播均值和协方差信息的方法。相比线性化,它更精确、更容易实现,且与线性化具有相同的计算量级。本文回顾了UT的动机、发展、使用及其意义。

关键词 — 估计,卡尔曼滤波,非线性系统,目标跟踪

这篇论文奠定了UKF的使用先河，论文比较复杂，如果之后有时间再详细推导和讲解，今天我讲下如何使用和论文关键思路及公式分析。

论文的第一句是:

The extended Kalman filter (EKF) is probably the most widely used estimation algorithm for nonlinear systems.

扩展卡尔曼滤波(EKF)可能是应用最广泛的非线性系统估计算法。

这里提到了EKF,它是卡尔曼滤波在非线性系统上的扩展。那么什么是卡尔曼滤波呢?

卡尔曼滤波是一种用于估计线性高斯系统状态的最优滤波算法。它分为两个步骤:预测和更新。

• 预测步骤:根据上一时刻的最优估计和输入,预测当前时刻的状态。
• 更新步骤:根据当前时刻的观测值,更新状态的估计。

预测和更新可以用下列公式表示:

预测:
$$\begin{array}{rl}{\hat{x}}_{k|k-1}& =A{\hat{x}}_{k-1|k-1}+B{u}_k\\ P_{k|k-1}& =A{P}_{k-1|k-1}{A}^T+Q\end{array}$$

更新:
$$\begin{array}{rl}{K}_k& =P_{k|k-1}{H}^T(H{P}_{k|k-1}{H}^T+R{)}^{-1}\\ {\hat{x}}_{k|k}& ={\hat{x}}_{k|k-1}+K_k(z_k-H{\hat{x}}_{k|k-1})\\ P_{k|k}& =(I-K_{k}H)P_{k|k-1}\end{array}$$

其中,$\hat{x}$是状态估计值,$P$是估计协方差,$A$是状态转移矩阵,$B$是控制矩阵,$Q$是过程噪声协方差,$H$是观测矩阵,$R$是观测噪声协方差,$K$是卡尔曼增益。下标$k|k-1$表示利用$k-1$时刻的信息对$k$时刻的量进行估计。

但是,卡尔曼滤波只适用于线性系统。现实中很多系统都是非线性的,比如机器人的运动方程、卫星的轨道方程等。对于非线性系统,状态转移和观测方程可以写成:

$$\begin{array}{rl}{x}_k& =f(x_{k-1},u_k)+w_k\\ z_k& =h(x_k)+v_k\end{array}$$

其中$f$是非线性状态转移函数,$h$是非线性观测函数,$w$和$v$分别是过程噪声和观测噪声。

EKF的思路就是,在每个时刻,用一阶泰勒展开来近似非线性函数,从而将非线性问题转化为线性问题。

假设在$k-1$时刻的状态估计为${\hat{x}}_{k-1|k-1}$,将$f$在该点处泰勒展开,并忽略高阶项,得到:

$$f(x_{k-1},u_k)\approx f({\hat{x}}_{k-1|k-1},u_k)+{\frac{\partial f}{\partial x}|}_{{\hat{x}}_{k-1|k-1},u_k}(x_{k-1}-{\hat{x}}_{k-1|k-1})$$

定义$A_k={\frac{\partial f}{\partial x}|}_{{\hat{x}}_{k-1|k-1},u_k}$为状态转移矩阵的近似,则预测步骤近似为:

$$\begin{array}{rl}{\hat{x}}_{k|k-1}& =f({\hat{x}}_{k-1|k-1},u_k)\\ P_{k|k-1}& =A_k{P}_{k-1|k-1}{A}_k^T+Q_k\end{array}$$

类似地,更新步骤近似为:

$$\begin{array}{rl}{K}_k& =P_{k|k-1}{H}_k^T(H_k{P}_{k|k-1}{H}_k^T+R_k{)}^{-1}\\ {\hat{x}}_{k|k}& ={\hat{x}}_{k|k-1}+K_k(z_k-h({\hat{x}}_{k|k-1}))\\ P_{k|k}& =(I-K_k{H}_k)P_{k|k-1}\end{array}$$

其中$H_k={\frac{\partial h}{\partial x}|}_{{\hat{x}}_{k|k-1}}$。

下面是EKF算法的一个简单实现:

def EKF(f, h, Q, R, x0, P0, z): 
    """
    f: 状态转移函数
    h: 观测函数
    Q: 过程噪声协方差  
    R: 观测噪声协方差
    x0: 初始状态   
    P0: 初始协方差
    z: 观测序列
    """
    n = len(x0) 
    K = len(z)
    
    xhat = np.zeros((n, K))
    xhat[:,0] = x0 
    P = P0
    
    for k in range(1, K):
        # 预测
        xhat_pri = f(xhat[:,k-1]) 
        F = jacobian(f, xhat[:,k-1])
        P_pri = F@P@F.T + Q
        
        # 更新 
        H = jacobian(h, xhat_pri)
        K = P_pri@H.T@np.linalg.inv(H@P_pri@H.T+R)
        xhat[:,k] = xhat_pri + K@(z[k] - h(xhat_pri)) 
        P = (np.eye(n)-K@H)@P_pri
        
    return xhat

其中jacobian函数用于计算雅可比矩阵,这里不再赘述。

接下来,论文指出了EKF的局限性:

However, more than 35 years of experience in the estimation community has shown that is difficult to implement, difficult to tune, and only reliable for systems that are almost linear on the time scale of the updates.

然而,估计领域35年的经验表明,EKF难以实现,难以调试,只对更新时间尺度上近似线性的系统可靠。

EKF主要有以下问题:

1. 线性化误差。泰勒展开截断了高阶项,当系统高度非线性时,近似误差会很大,甚至导致滤波发散。
2. 计算雅可比矩阵很困难。对于复杂的非线性函数,求解雅可比矩阵是个费时费力的过程。
3. 不适用于不光滑的函数。EKF要求函数必须可导,对于不连续、有跳变的函数无能为力。

为了克服这些问题,作者提出了Unscented Transform (UT)。

The unscented transformation (UT) was developed to address the deficiencies of linearization by providing a more direct and explicit mechanism for transforming mean and covariance information.

Unscented变换(UT)通过提供一种更直接、更显式的均值和协方差转换机制,来解决线性化的缺陷。

UT的基本思想是:用一组确定性采样点(sigma points)来近似概率分布,并通过非线性变换直接传播这些采样点,用其均值和协方差来近似非线性变换后的真实分布。

假设我们要估计一个$n$维随机变量$x$经过非线性变换$y=g(x)$后的均值和协方差。已知$x$的均值$\bar{x}$和协方差$P_x$。UT的具体步骤如下:

1. 确定性采样:在$x$的分布中选取$2n+1$个sigma points ${\mathcal{X}}_i$及其权重$W_i$。其中
   $$\begin{array}{rl}{\mathcal{X}}_0& =\bar{x}\\ {\mathcal{X}}_i& =\bar{x}+{\left(\sqrt{(n+\lambda )P_x}\right)}_i,i=1,\dots ,n\\ {\mathcal{X}}_{i+n}& =\bar{x}-{\left(\sqrt{(n+\lambda )P_x}\right)}_i,i=1,\dots ,n\end{array}$$
   其中${\left(\sqrt{(n+\lambda )P_x}\right)}_i$表示矩阵平方根的第$i$列。权重为
   $$\begin{array}{rl}{W}_0& =\frac{\lambda }{n+\lambda }\\ W_i& =\frac{1}{2(n+\lambda )},i=1,\dots ,2n\end{array}$$
   $\lambda$是一个缩放参数,通常取$\lambda =3-n$。

2. 传播:将每个sigma point通过非线性函数传播
   $${\mathcal{Y}}_i=g({\mathcal{X}}_i),i=0,\dots ,2n$$

3. 计算均值和协方差:
   $$\begin{array}{rl}\bar{y}& =\sum _{i=0}^{2n}{W}_i{\mathcal{Y}}_i\\ P_y& =\sum _{i=0}^{2n}{W}_i({\mathcal{Y}}_i-\bar{y})({\mathcal{Y}}_i-\bar{y}{)}^T\end{array}$$

UT的核心思想是用一组确定性采样点(sigma points)来近似概率分布,并通过非线性变换直接传播这些采样点,用其均值和协方差来近似非线性变换后的真实分布。

假设我们要估计一个$n$维随机变量$x$经过非线性变换$y=g(x)$后的均值和协方差。已知$x$服从均值为$\bar{x}$,协方差为$P_x$的概率分布。

1. Sigma Points的选取

首先,我们要从$x$的分布中选取一组代表性的采样点。直观地,这些点应该captures分布的重要特征,如均值、方差、偏度等。一种直接的想法是用$\bar{x}$加减一个与$P_x$有关的扰动项。

UT采用对称采样的方式,在$\bar{x}$周围对称地选取$2n$个点,外加一个位于$\bar{x}$处的点,共$2n+1$个sigma points:

$$\begin{array}{rl}{\mathcal{X}}_0& =\bar{x}\\ {\mathcal{X}}_i& =\bar{x}+{\left(\sqrt{(n+\lambda )P_x}\right)}_i,i=1,\dots ,n\\ {\mathcal{X}}_{i+n}& =\bar{x}-{\left(\sqrt{(n+\lambda )P_x}\right)}_i,i=1,\dots ,n\end{array}$$

其中${\left(\sqrt{(n+\lambda )P_x}\right)}_i$表示矩阵$\sqrt{(n+\lambda )P_x}$的第$i$列。这个矩阵可以通过Cholesky分解等方法求得。它与$P_x$有关,反映了概率分布的形状。乘以$\sqrt{n+\lambda }$是为了让点的分布更分散一些,体现非高斯的特征。

$\lambda$是一个缩放参数,用于控制采样点的散布程度。文中提到通常取$\lambda =3-n$,这是基于高斯分布的启发。对于高斯分布,$\lambda =3-n$时,采样点恰好落在$.8\sigma$处(即概率椭球面上)。

2. 权重的计算

为了用采样点们来近似原分布,我们需要为每个点分配一个权重$W_i$,使得加权和能够逼近概率积分。UT的权重选取方式为:

$$\begin{array}{rl}{W}_0& =\frac{\lambda }{n+\lambda }\\ W_i& =\frac{1}{2(n+\lambda )},i=1,\dots ,2n\end{array}$$

这些权重有如下特点:

1. 权重之和为1: ${\sum }_{i=0}^{2n}{W}_i=1$。这是概率的基本性质。

2. 中心点${\mathcal{X}}_0$的权重$W_0$与$\lambda$有关。$\lambda$越大,$W_0$越大,中心点的影响越大。当$\lambda =3-n$时,$W_0$的取值在0到1之间。

3. 其他点的权重$W_i(i\ne 0)$相等,都为$\frac{1}{2(n+\lambda )}$。这反映了UT的对称采样策略。

事实上,上述采样点和权重的选取方式,可以使得采样点的均值和协方差与原分布完全一致,即:

$$\bar{x}=\sum _{i=0}^{2n}{W}_i{\mathcal{X}}_i$$

$$P_x=\sum _{i=0}^{2n}{W}_i({\mathcal{X}}_i-\bar{x})({\mathcal{X}}_i-\bar{x}{)}^T$$

这保证了用采样点能够较好地逼近原分布。在文献[1]中有详细的证明。

3. 传播与重构

选取采样点后,将其通过非线性函数$g(\cdot )$传播:

$${\mathcal{Y}}_i=g({\mathcal{X}}_i),i=0,\dots ,2n$$

然后,用采样点的加权均值和协方差来近似$y$的分布:

$$\begin{array}{rl}\bar{y}& =\sum _{i=0}^{2n}{W}_i{\mathcal{Y}}_i\\ P_y& =\sum _{i=0}^{2n}{W}_i({\mathcal{Y}}_i-\bar{y})({\mathcal{Y}}_i-\bar{y}{)}^T\end{array}$$

可以证明,这种近似在$y=g(x)$为线性函数时是精确的;在$g(x)$非线性时,它在$\bar{x}$处做了高斯近似,精度为三阶。相比线性化方法(一阶),UT能够更好地捕捉非线性的影响。且UT只需要非线性函数本身,无需计算导数,实现更简单。

4. 一些符号说明

• $n$: 状态维度
• $\bar{x}$: 状态的先验均值
  -$P_x$:状态的先验协方差矩阵
• ${\mathcal{X}}_i$: 第$i$个sigma point
• $W_i$: 第$i$个sigma point的权重
• $\lambda$: UT的缩放参数
• ${\mathcal{Y}}_i$: 第$i$个sigma point经非线性函数传播后的值
• $\bar{y}$: 近似的后验均值
• $P_y$: 近似的后验协方差矩阵

以上就是UT中采样与权重计算的主要推导过程与符号解释。这种确定性采样避免了随机采样的不确定性,且对非线性、非高斯的情况有更好的逼近效果,是一种精度与效率兼顾的方法。

参考文献

[1] Julier, S. J., & Uhlmann, J. K. (2004). Unscented filtering and nonlinear estimation. Proceedings of the IEEE, 92(3), 401-422.

可以看到,UT只需要非线性函数本身,而不需要计算雅可比矩阵。且sigma points是确定性采样得到的,不像Monte Carlo方法那样引入随机性。另外,通过调整$\lambda$还可以部分考虑非高斯分布的影响。

下面给出UT的一个简单实现:

def UT(f, x, P, kappa):
    """
    f: 非线性函数 
    x: 均值
    P: 协方差
    kappa: 缩放参数
    """
    n = len(x)
    lambda_ = 3 - n
    
    # 计算sigma points
    U = cholesky((n+lambda_)*P)
    X = np.tile(x, (1,2*n+1)) 
    X[:,1:n+1] += U
    X[:,n+1:] -= U
    W = np.ones(2*n+1)/(2*(n+lambda_)) 
    W[0] = lambda_/(n+lambda_)

    # 传播  
    Y = f(X) 
    
    # 计算均值和协方差
    y = np.sum(W[:,None]*Y, axis=0)
    Py = np.sum(W[:,None]*(Y-y)@(Y-y).T, axis=0)
    
    return y, Py

接下来,论文用一个简单的例子说明了UT的优越性。考虑一个二维到二维的非线性变换:

$$\begin{array}{rl}{y}_1& =x_1\\ y_2& =x_1^2+x_2^2\end{array}$$

假设$x$服从均值为$[0,0{]}^T$,协方差为$\text{diag}(0.3,0.1)$的高斯分布。变换后$y$的真实均值和协方差可以解析求得:
$$\begin{array}{rl}\mathrm{E}[y_1]& =0\\ \mathrm{E}[y_2]& =0.4\\ \mathrm{Cov}[y]& =\left[\begin{array}{cc}0.09& 0\\ 0& 1.56\end{array}\right]\end{array}$$

而用EKF和UT估计的结果分别为:

• EKF:
  $$\bar{y}=\left[\begin{array}{c}0\\ 0\end{array}\right],P_y=\left[\begin{array}{cc}0.09& 0\\ 0& 0.49\end{array}\right]$$

• UT ($\kappa =2$):
  $$\bar{y}=\left[\begin{array}{c}0\\ 0.4\end{array}\right],P_y=\left[\begin{array}{cc}0.09& 0\\ 0& 0.77\end{array}\right]$$

可以看到,UT给出的均值和协方差估计都明显优于EKF。这是因为EKF只考虑了一阶泰勒展开,而UT隐含地考虑了高阶项的影响。

下面我们将UT应用到卡尔曼滤波中,就得到了Unscented Kalman Filter (UKF)。UKF的预测和更新步骤如下:

预测:

1. 根据${\hat{x}}_{k-1|k-1}$和$P_{k-1|k-1}$采样sigma点${\mathcal{X}}_{k-1}$。
2. 状态预测:
   $${\mathcal{X}}_{k|k-1}=f({\mathcal{X}}_{k-1},u_k)$$
   $$\begin{array}{rl}{\hat{x}}_{k|k-1}& =\sum _{i=0}^{2n}{W}_i^{(m)}{\mathcal{X}}_{i,k|k-1}\\ P_{k|k-1}& =\sum _{i=0}^{2n}{W}_i^{(c)}({\mathcal{X}}_{i,k|k-1}-{\hat{x}}_{k|k-1})({\mathcal{X}}_{i,k|k-1}-{\hat{x}}_{k|k-1}{)}^T+Q_k\end{array}$$

更新:

1. 观测预测:
   $${\mathcal{Z}}_{k|k-1}=h({\mathcal{X}}_{k|k-1})$$
   $$\begin{array}{rl}{\hat{z}}_{k|k-1}& =\sum _{i=0}^{2n}{W}_i^{(m)}{\mathcal{Z}}_{i,k|k-1}\\ P_{zz}& =\sum _{i=0}^{2n}{W}_i^{(c)}({\mathcal{Z}}_{i,k|k-1}-{\hat{z}}_{k|k-1})({\mathcal{Z}}_{i,k|k-1}-{\hat{z}}_{k|k-1}{)}^T+R_k\end{array}$$

2. 计算卡尔曼增益:
   $$P_{xz}=\sum _{i=0}^{2n}{W}_i^{(c)}({\mathcal{X}}_{i,k|k-1}-{\hat{x}}_{k|k-1})({\mathcal{Z}}_{i,k|k-1}-{\hat{z}}_{k|k-1}{)}^T$$
   $$K_k=P_{xz}{P}_{zz}^{-1}$$

3. 状态和协方差更新:
   $$\begin{array}{rl}{\hat{x}}_{k|k}& ={\hat{x}}_{k|k-1}+K_k(z_k-{\hat{z}}_{k|k-1})\\ P_{k|k}& =P_{k|k-1}-K_k{P}_{zz}{K}_k^T\end{array}$$

可以看到,UKF的结构与标准卡尔曼滤波非常相似,只是用UT替代了线性化的过程。下面给出UKF的一个简单实现:

def UKF(f, h, Q, R, x0, P0, z, kappa):
    """
    f: 状态转移函数
    h: 观测函数
    Q: 过程噪声协方差
    R: 观测噪声协方差
    x0: 初始状态
    P0: 初始协方差
    z: 观测序列
    kappa: UT的缩放参数
    """
    n = len(x0)
    lambda_ = 3-n
    K = len(z)
    
    xhat = np.zeros((n,K))
    xhat[:,0] = x0
    P = P0
    
    for k in range(1,K):
        # sigma点
        X,W = UT_sigma_points(xhat[:,k-1], P, lambda_) 

        # 状态预测
        Xf = f(X)
        xhat_pri = np.sum(W[None,:]*Xf,axis=1) 
        P_pri = np.sum(W[None,:]*(Xf-xhat_pri[:,None])@(Xf-xhat_pri[:,None]).T,axis=1) + Q

        # 观测预测
        Zp = h(Xf)
        z_pri = np.sum(W[None,:]*Zp,axis=1)
        Pzz = np.sum(W[None,:]*(Zp-z_pri[:,None])@(Zp-z_pri[:,None]).T,axis=1) + R
        
        # 卡尔曼增益 
        Pxz = np.sum(W[None,:]*(Xf-xhat_pri[:,None])@(Zp-z_pri[:,None]).T,axis=1)
        K = Pxz@np.linalg.inv(Pzz)

        # 更新
        xhat[:,k] = xhat_pri + K@(z[k]-z_pri)
        P -= K@Pzz@K.T

    return xhat    

其中UT_sigma_points函数根据均值和协方差生成sigma点,具体实现略。

原论文中使用这个目标跟踪的例子来展示UKF的性能。考虑一个匀速运动的目标,其状态为位置和速度$[x,\dot{x},y,\dot{y}{]}^T$,状态方程为:
$$\begin{array}{rl}{x}_{k+1}& =x_k+\Delta t\cdot {\dot{x}}_k\\ {\dot{x}}_{k+1}& ={\dot{x}}_k\\ y_{k+1}& =y_k+\Delta t\cdot {\dot{y}}_k\\ {\dot{y}}_{k+1}& ={\dot{y}}_k\end{array}$$

雷达测量到的是目标的斜距$r$和方位角$\theta$,观测方程为:
$$\begin{array}{rl}{r}_k& =\sqrt{x_k^2+y_k^2}+v_{r,k}\\ {\theta }_k& =\arctan (\frac{y_k}{x_k})+v_{\theta ,k}\end{array}$$

其中$v_r\sim \mathcal{N}(0,{\sigma }_r^2),v_{\theta }\sim \mathcal{N}(0,{\sigma }_{\theta }^2)$。

我们生成一组模拟数据,目标从$[1000,300,1000,0]$出发,匀速运动20秒。雷达每秒测量一次,距离和角度的标准差分别为10米和0.1弧度。

分别用EKF和UKF进行目标跟踪,结果UKF的估计结果更接近真实轨迹,尤其是在转弯处。(具体数据见原论文)

以上就是我对Unscented Kalman Filter的理解和讲解,核心就是用Unscented Transform来近似非线性分布,取代EKF中的线性化过程。UT通过确定性采样捕捉分布的重要特征,估计精度高,且实现简单,是一个应对非线性滤波问题的利器。

Q&A

UKF中的UT采样过程

假设我们要估计一个二维状态量$\mathbf{x}=[x_1,x_2{]}^T$经过非线性函数$\mathbf{y}=\mathbf{f}(\mathbf{x})$变换后的均值和协方差。已知$\mathbf{x}$服从高斯分布,均值为$\bar{\mathbf{x}}=[0,0{]}^T$,协方差为${\mathbf{P}}_x=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]$。

UT采样的步骤如下:

1. 确定采样点的数量。对于n维状态量,我们通常选取$2n+1$个sigma点。这里$n=2$,所以我们将选取5个sigma点。

2. 计算采样点。对于高斯分布,我们按以下方式选取sigma点(其中$\lambda$是缩放参数,通常取$\lambda =3-n$):

$$\begin{array}{rl}{\mathcal{X}}_0& =\bar{\mathbf{x}}\\ {\mathcal{X}}_i& =\bar{\mathbf{x}}+{\left(\sqrt{(n+\lambda ){\mathbf{P}}_x}\right)}_i,i=1,\dots ,n\\ {\mathcal{X}}_{i+n}& =\bar{\mathbf{x}}-{\left(\sqrt{(n+\lambda ){\mathbf{P}}_x}\right)}_i,i=1,\dots ,n\end{array}$$

其中${\left(\sqrt{(n+\lambda ){\mathbf{P}}_x}\right)}_i$表示矩阵$\sqrt{(n+\lambda ){\mathbf{P}}_x}$的第$i$列。在这个例子中,

$$\sqrt{(n+\lambda ){\mathbf{P}}_x}=\sqrt{3}\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]=\left[\begin{array}{cc}\sqrt{3}& 0\\ 0& \sqrt{3}\end{array}\right]$$

因此,五个sigma点为:

$$\begin{array}{rl}{\mathcal{X}}_0& =[0,0{]}^T\\ {\mathcal{X}}_1& =[\sqrt{3},0{]}^T\\ {\mathcal{X}}_2& =[0,\sqrt{3}{]}^T\\ {\mathcal{X}}_3& =[-\sqrt{3},0{]}^T\\ {\mathcal{X}}_4& =[0,-\sqrt{3}{]}^T\end{array}$$

3. 计算权重。每个sigma点都有一个权重,用于计算变换后的均值和协方差。权重计算如下:

$$\begin{array}{rl}{W}_0^{(m)}& =\frac{\lambda }{n+\lambda }\\ W_0^{(c)}& =\frac{\lambda }{n+\lambda }+(1-{\alpha }^2+\beta )\\ W_i^{(m)}=W_i^{(c)}& =\frac{1}{2(n+\lambda )},i=1,\dots ,2n\end{array}$$

其中上标$(m)$表示用于计算均值的权重,$(c)$表示用于计算协方差的权重。在这个例子中,如果取$\alpha =1,\beta =2$,权重为:

$$\begin{array}{rl}{W}_0^{(m)}& =\frac{1}{3}\\ W_0^{(c)}& =\frac{1}{3}+(1-1^2+2)=\frac{5}{3}\\ W_1^{(m)}=W_2^{(m)}=W_3^{(m)}=W_4^{(m)}& =\frac{1}{6}\\ W_1^{(c)}=W_2^{(c)}=W_3^{(c)}=W_4^{(c)}& =\frac{1}{6}\end{array}$$

4. 传播sigma点。将每个sigma点通过非线性函数$\mathbf{f}(\cdot )$:

$${\mathcal{Y}}_i=\mathbf{f}({\mathcal{X}}_i),i=0,\dots ,4$$

5. 计算均值和协方差。用加权和来近似$\mathbf{y}$的均值和协方差:

$$\begin{array}{rl}\bar{\mathbf{y}}& =\sum _{i=0}^4{W}_i^{(m)}{\mathcal{Y}}_i\\ {\mathbf{P}}_y& =\sum _{i=0}^4{W}_i^{(c)}({\mathcal{Y}}_i-\bar{\mathbf{y}})({\mathcal{Y}}_i-\bar{\mathbf{y}}{)}^T\end{array}$$

这就是UKF中的UT采样过程。它通过确定性采样捕捉了原始高斯分布的均值和协方差信息,然后用采样点的加权和来近似非线性变换后的均值和协方差。与EKF中的线性化方法相比,UT能达到更高的估计精度,且无需计算雅可比矩阵。

$\alpha$和$\beta$是UT中的调节参数:

• $\alpha$决定sigma点围绕均值的散布程度,通常取一个很小的正值,如0.001。它影响了协方差权重$W_0^{(c)}$的计算。
• $\beta$是用来融合先验知识的参数。对于高斯分布,$\beta =2$是最优的。

现在,让我们完整地实现这个例子。假设非线性函数为:

$$\mathbf{f}(\mathbf{x})=\left[\begin{array}{c}{x}_1^2+x_2^2\\ \sin (x_1)+\cos (x_2)\end{array}\right]$$

我们已经计算了五个sigma点${\mathcal{X}}_i$和权重$W_i^{(m)},W_i^{(c)}$。接下来:

1. 传播sigma点:

$$\begin{array}{rl}{\mathcal{Y}}_0& =\mathbf{f}({\mathcal{X}}_0)=[0,1{]}^T\\ {\mathcal{Y}}_1& =\mathbf{f}({\mathcal{X}}_1)=[3,\sin (\sqrt{3}){]}^T\\ {\mathcal{Y}}_2& =\mathbf{f}({\mathcal{X}}_2)=[3,\cos (\sqrt{3}){]}^T\\ {\mathcal{Y}}_3& =\mathbf{f}({\mathcal{X}}_3)=[3,\sin (-\sqrt{3}){]}^T\\ {\mathcal{Y}}_4& =\mathbf{f}({\mathcal{X}}_4)=[3,\cos (-\sqrt{3}){]}^T\end{array}$$

5. 计算均值和协方差:

$$\begin{array}{rl}\bar{\mathbf{y}}& =\sum _{i=0}^4{W}_i^{(m)}{\mathcal{Y}}_i\\ & =\frac{1}{3}[0,1{]}^T+\frac{1}{6}[3,\sin (\sqrt{3}){]}^T+\frac{1}{6}[3,\cos (\sqrt{3}){]}^T\\ & +\frac{1}{6}[3,\sin (-\sqrt{3}){]}^T+\frac{1}{6}[3,\cos (-\sqrt{3}){]}^T\\ & \approx [2,0.075{]}^T\end{array}$$

$$\begin{array}{rl}{\mathbf{P}}_y& =\sum _{i=0}^4{W}_i^{(c)}({\mathcal{Y}}_i-\bar{\mathbf{y}})({\mathcal{Y}}_i-\bar{\mathbf{y}}{)}^T\\ & \approx \left[\begin{array}{cc}2.333& -0.211\\ -0.211& 1.296\end{array}\right]\end{array}$$

现在我们得到了$\mathbf{y}$的均值$\bar{\mathbf{y}}$和协方差${\mathbf{P}}_y$的估计。这就完成了UKF的预测步骤。

接下来,我们可以将$\bar{\mathbf{y}}$和${\mathbf{P}}_y$用于UKF的更新步骤:

6. 计算卡尔曼增益:

$${\mathbf{K}}_k={\mathbf{P}}_{xy}{\mathbf{P}}_y^{-1}$$

其中${\mathbf{P}}_{xy}$是$\mathbf{x}$和$\mathbf{y}$的交叉协方差,可以用类似的UT方法估计。

7. 更新状态估计:

$${\hat{\mathbf{x}}}_k={\hat{\mathbf{x}}}_{k|k-1}+{\mathbf{K}}_k({\mathbf{y}}_k-{\bar{\mathbf{y}}}_k)$$

其中${\hat{\mathbf{x}}}_{k|k-1}$是先验状态估计,${\mathbf{y}}_k$是实际测量值。

8. 更新估计协方差:

$${\mathbf{P}}_k={\mathbf{P}}_{k|k-1}-{\mathbf{K}}_k{\mathbf{P}}_y{\mathbf{K}}_k^T$$

其中${\mathbf{P}}_{k|k-1}$是先验估计协方差。

这就是完整的UKF过程。预测步骤用UT估计非线性变换后的均值和协方差,更新步骤则将预测值与实际测量值融合,更新状态估计。

希望这个详细的例子能帮助你更清晰地理解UKF和UT采样的过程。