1 前言
- 前文介绍了扩展卡尔曼滤波,它对非线性函数线性化的方法是泰勒展开式,但这不是唯一线性化的方法
- 本文介绍的无迹卡尔曼滤波(unscented Kalman filter, UKF)是应用无迹变换进行线性化
- 无迹变换要比泰勒展开式更加准确,所以对于非线性滤波效果更好
2 通过无迹变换实现线性化
- 这里不做公式推导和证明,仅给出算法所需无迹变换实现线性化的公式
- 无迹变换是通过从高斯分布中提取 σ \sigma σ点( χ [ i ] \chi^{[i]} χ[i]),再对这些 σ \sigma σ点进行非线性变换,具体方法见第3部分
- 对于 n n n维高斯分布要提取 2 n + 1 2n+1 2n+1个 σ \sigma σ点( χ [ i ] \chi^{[i]} χ[i])
- 这些 σ \sigma σ点( χ [ i ] \chi^{[i]} χ[i])位于均值附近,对称分布:
χ [ 0 ] = μ χ [ i ] = μ + ( ( n + λ ) Σ ) i i = 1 , ⋯ , n χ [ i ] = μ − ( ( n + λ ) Σ ) i − n i = n + 1 , ⋯ , 2 n 式中: λ = α 2 ( n + k ) − n ( α , k :为尺度参数,决定 σ 点距离均值的远近 ) 注意: ( n + λ ) Σ 是矩阵开平方,例如: B = A ⇒ A T A = B , matlab函数为sqrtm() \begin{aligned} \chi^{[0]} &= \mu\\ \chi^{[i]} &= \mu + \left (\sqrt{(n+\lambda)\Sigma}\right )_i ~~ i = 1, \cdots, n\\ \chi^{[i]} &= \mu - \left (\sqrt{(n+\lambda)\Sigma}\right )_{i-n} ~~ i = n+1, \cdots, 2n\\ \end{aligned}\\ \text{式中:}\lambda = \alpha^2(n+k) - n~~~~(\alpha,k\text{:为尺度参数,决定$\sigma$点距离均值的远近})\\ \text{注意:$\sqrt{(n+\lambda)\Sigma}$是矩阵开平方,例如:$\sqrt{B} = A \Rightarrow A^TA = B$, matlab函数为sqrtm()} χ[0]χ[i]χ[i]=μ=μ+((n+λ)Σ)i i=1,⋯,n=μ−((n+λ)Σ)i−n i=n+1,⋯,2n式中:λ=α2(n+k)−n (α,k:为尺度参数,决定σ点距离均值的远近)注意:(n+λ)Σ是矩阵开平方,例如:B=A⇒ATA=B, matlab函数为sqrtm() - 有两个权重:
- w m [ i ] w_m^{[i]} wm[i]:用于计算均值
- w c [ i ] w_c^{[i]} wc[i]:用于计算方差
w m [ 0 ] = λ n + λ w c [ 0 ] = λ n + λ + ( 1 − α 2 + β ) w m [ i ] = w c [ i ] = 1 2 ( n + λ ) i = 1 , ⋯ , 2 n \begin{aligned} w_m^{[0]} &= \frac{\lambda}{n + \lambda}\\ w_c^{[0]} &= \frac{\lambda}{n + \lambda} + (1 - \alpha^2 + \beta)\\ w_m^{[i]} &= w_c^{[i]} = \frac{1}{2(n + \lambda)} ~~~~i = 1,\cdots, 2n \end{aligned} wm[0]wc[0]wm[i]=n+λλ=n+λλ