一个非常非常简单的UKF入门程序实例

一曲冷凌霜 提交于 2020-02-19 02:39:03

UKF

UKF为Unscented Kalman Filter简写,中文可翻译为无损卡尔曼滤波,本滤波是为了解决非线性问题存在的(也可用于线性模型),即结合了UT变换和标准卡尔曼滤波的一种方法。

UKF原理的通俗解释

其实对于状态的一步递推和量测的一步递推,是不存在非线性带来的难题的。考虑以下非线性模型:
xk+1=f(xk)+wx_{k+1}=f(x_{k})+w
yk+1=h(xk)+vy_{k+1}=h(x_{k})+v
可以很简单的通过方程f(x)f(x)h(x)h(x)求得一步递推值。但是,对于误差特性的传递却不能通过非线性方程求取了。
例如,对于线性方程来说,误差的传递也是线性的,非线性模型却比较麻烦。EKF的思路是将模型线性化,还是通过经典卡尔曼滤波的思路解决问题。UKF另辟蹊径,通多采样的方法,将采样点通多非线性模型一一传递,然后计算传递后的采样点映射点的统计误差!
于是一切有解了。

一个很简单的模型

下边是我们此次试验的模型,首先它是一维的,易于直观理解其原理,另外可以发现此模型的状态方程式线性的,观测方程是非线性的,取此测试模型是刻意的。这样可以体现UKF另一个好处,即UKF和标准卡尔曼滤波,甚至EKF可以“混用”,是不是很好玩呢。
xk+1=xk x_{k+1} = x_{k}

yk+1=xk+1y_{k+1} = \sqrt{x_{k+1}}

python 代码

#!/usr/bin/env python
# coding: utf-8

import math
from math import sqrt
import numpy as np
import matplotlib.pyplot as plt

_ls_x_true = []
_ls_y = []
_ls_x_noise = []
DATA_ROWS = 360
X_true = 10
for i in range(DATA_ROWS):
    _ls_x_true.append(X_true)
    noise_s = np.random.normal(loc=0, scale=2.5)
    x_with_noise = X_true+noise_s
    _ls_x_noise.append(x_with_noise)
    _ls_y.append(sqrt(x_with_noise))


_Xs = _ls_y[0]**2
_P = 0.1
_Q = 0.05
R = 2

kapa = 0.2
alpha = 0.9
L = 1
beta = 2
lamb = alpha*alpha*(L+kapa)-L
w_c = 0.5/(alpha*alpha*(L+kapa))
w_m0 = lamb/(alpha*alpha*(L+kapa))
w_c0 = lamb/(alpha*alpha*(L+kapa))+1-alpha**2+beta

ls_x_hat = []
ls_P = []

for i in range(DATA_ROWS):
    # predict
    _Xs = _Xs
    _P = _P + _Q
    
    # correct
    x0 = _Xs
    x1 = _Xs + sqrt((L+lamb)*_P)
    x2 = _Xs - sqrt((L+lamb)*_P)

    y0 = sqrt(x0)
    y1 = sqrt(x1)
    y2 = sqrt(x2)
    
    y_hat = w_m0*y0 + w_c*y1+ w_c*y2
    y = _ls_y[i]
    s_hat = w_c0*(y_hat-y0)*(y_hat-y0)+w_c*(y_hat-y1)*(y_hat-y1)+w_c*(y_hat-y2)*(y_hat-y2)
    s_hat = s_hat + R
    
    C_sz = (x0-_Xs)*(y0-y_hat)*w_c0+(x1-_Xs)*(y1-y_hat)*w_c+(x2-_Xs)*(y2-y_hat)*w_c
    
    
    K_k = C_sz/s_hat
    _Xs = _Xs + K_k*(y - y_hat)
    
    _P = _P - K_k*s_hat*K_k
    ls_P.append(_P)
    
    ls_x_hat.append(_Xs)

plt.plot(ls_x_hat,label='x_hat')
plt.plot(_ls_x_true,label='x_true')
plt.plot(_ls_x_noise,label='x_noise')
plt.legend()
plt.grid(True)
plt.grid(linestyle='--')
plt.show()

plt.plot(ls_P,label='P')
plt.legend()
plt.grid(True)
plt.grid(linestyle='--')
plt.show()

实验结果

ukf_states
UKF_P

易学教程内所有资源均来自网络或用户发布的内容,如有违反法律规定的内容欢迎反馈
该文章没有解决你所遇到的问题?点击提问,说说你的问题,让更多的人一起探讨吧!