EKF UKF

　　最初设计的卡尔曼滤波主要针对于线性系统，然而现实世界并不存在真正意义上的线性系统。

 

　　因此，为了适应实际应用场景，一种对状态量进行局部线性化的扩展卡尔曼滤波（Extended Kalman Filter，EKF）被提出。该方案在最小方差准则条件下的次优滤波器，因其结构简单、计算量小等优点，被广泛应用于各行各业中。然而，当系统的非线性化或非高斯的程度比较严重时，对状态向量进行局部线性化仍然不能很好地解决系统非线性问题。

 

　　此时，非线性估计方法应运而生，例如无迹卡尔曼滤波（Unscented Kalman Filter,UKF）和粒子滤波（Particle Filter, PF）。

 

　　其中，UKF沿用卡尔曼滤波的基本框架，仍然基于系统噪声为高斯白噪声的假设条件，使用无迹变换方法来处理均值和协方差的非线性传播。其基本思想是放弃对非线性函数进行直接近似，而使用一系列的确定样本逼近或者近似状态的后验概率密度分布。然而，当系统的非线性非常强时，或系统噪声为非高斯时，UKF仍旧无法阻止系统发散，此时可以使用完全的非线性滤波器PF 进行状态估计。

 

　　PF是指通过在状态空间中寻找一组传播的随机样本（即粒子）对概率密度函数进行近似，使用样本均值替代积分运算，从而获得系统状态的最小方差估计的过程。

 

 

https://zhuanlan.zhihu.com/p/41767489   概率机器人——扩展卡尔曼滤波、无迹卡尔曼滤波

 https://filterpy.readthedocs.io/en/latest/index.html      FilterPy  python 库的使用

无迹卡尔曼滤波-1

无迹卡尔曼滤波-2

 sigma点选取代码如下：

#因此，一共有三个常数(α,β,κ)需要我们来设定。根据其它参考资料，选择参数的经验为：β=2 is a good choice for Gaussian problems, κ=3−n is a good choice for κ , and 0≤α≤1 is an appropriate choice for α.
#α越大，第一个sigma点(平均值处)占的权重越大，并且其它sigma点越远离均值。如下图所示，对2维随机变量选取5个sigma点，点的大小代表权重，由感性认识可知sigma点离均值越远其权重应该越小。
# -*- coding: utf-8 -*-
import numpy as np
from filterpy.kalman import MerweScaledSigmaPoints as SigmaPoints

mean = 0    # 均值
cov =  1    # 方差

points = SigmaPoints(n=1, alpha=0.1, beta=2.0, kappa=1.0)
sigmas = points.sigma_points(mean, cov)

print(points.Wm, points.Wc)    # 计算均值和方差的权值
print(sigmas)     # sigma点的坐标

　　

 其中代码仿真   如下：

 

# 2维正态分布的例子。从一个服从二维正态分布的随机变量中随机选取1000个点
# 下面使用unscented transform方法近似计算非线性变换后随机变量的期望，并与直接从1000个点计算出的期望值对比
# -*- coding: utf-8 -*-
import numpy as np
from scipy import stats
import matplotlib.pyplot as plt
from numpy.random import multivariate_normal
from filterpy.kalman import unscented_transform
from filterpy.kalman import MerweScaledSigmaPoints as SigmaPoints

# 非线性变换函数
def f_nonlinear_xy(x, y):
    return np.array([x + y, 0.1*x**2 + y**2])

def plot1(xs, ys):
    xs = np.asarray(xs)
    ys = np.asarray(ys)
    xmin = xs.min()
    xmax = xs.max()
    ymin = ys.min()
    ymax = ys.max()
    values = np.vstack([xs, ys])
    kernel = stats.gaussian_kde(values)
    X, Y = np.mgrid[xmin:xmax:100j, ymin:ymax:100j]
    positions = np.vstack([X.ravel(), Y.ravel()])
    Z = np.reshape(kernel.evaluate(positions).T, X.shape)
    plt.imshow(np.rot90(Z),cmap=plt.cm.Greys,extent=[xmin, xmax, ymin, ymax])
    plt.plot(xs, ys, 'k.', markersize=2)
    plt.xlim(-20, 20)
    plt.ylim(-20, 20)

def plot2(xs, ys, f, mean_fx):
    fxs, fys = f(xs, ys)    # 将采样点进行非线性变换
    computed_mean_x = np.average(fxs)
    computed_mean_y = np.average(fys)
    plt.subplot(121)
    plt.grid(False)
    plot1(xs, ys)
    plt.subplot(122)
    plt.grid(False)
    plot1(fxs, fys)
    plt.scatter(fxs, fys, marker='.', alpha=0.01, color='k')
    plt.scatter(mean_fx[0], mean_fx[1], marker='o', s=100, c='r', label='UT_mean')
    plt.scatter(computed_mean_x, computed_mean_y, marker='*',s=120, c='b', label='mean')
    plt.ylim([-10, 200])
    plt.xlim([-100, 100])
    plt.legend(loc='best', scatterpoints=1)
    print ('Difference in mean x={:.3f}, y={:.3f}'.format(
           computed_mean_x-mean_fx[0], computed_mean_y-mean_fx[1]))

# -------------------------------------------------------------------------------------------
mean = [0, 0]               # Mean of the N-dimensional distribution.
cov = [[32, 15], [15, 40]]  # Covariance matrix of the distribution.

# create sigma points(2n+1个sigma点)
# uses 3 parameters to control how the sigma points are distributed and weighted
points = SigmaPoints(n=2, alpha=.1, beta=2., kappa=1.)
Wm = points.Wm
Wc = points.Wc
sigmas = points.sigma_points(mean, cov)

# pass through nonlinear function
sigmas_f = np.empty((5, 2))
for i in range(5):
    sigmas_f[i] = f_nonlinear_xy(sigmas[i, 0], sigmas[i ,1])

# use unscented transform to get new mean and covariance
ukf_mean, ukf_cov = unscented_transform(sigmas_f, Wm, Wc)

# generate random points
xs, ys = multivariate_normal(mean, cov, size=1000).T  # 从二维随机变量的正态分布中产生1000个数据点
plot2(xs, ys, f_nonlinear_xy, ukf_mean)

# 画sigma点
plt.xlim(-30, 30); plt.ylim(0, 90)
plt.subplot(121)
plt.scatter(sigmas[:,0], sigmas[:,1], c='r', s=30)
plt.show()

 

 结果如下：

 

Difference in mean x=-0.144, y=-0.204