无迹卡尔曼滤波器中的负协方差矩阵

问题描述

我想为非线性问题实现Unscented Kalman filter(UKF)方法；我设置了所有初始值，例如初始均值向量和初始协方差矩阵。问题的维度是 8。 下面是实现代码；我不知道为什么代码末尾的新协方差矩阵包含一些负参数。这些负参数为该方法的下一次迭代带来了问题。

#pip install filterpy
import numpy as np
from filterpy.kalman import unscented_transform,MerweScaledSigmaPoints

mean_zero = np.array([ 27.25,14.39,4.459,27.65,6.27,23.653,1.2,0.21 ])
P_zero = np.diagflat((0.05*mean_zero)**2)
#initial parameters
T_2 =10
T_3 = 8
dt = 1
Q_1 = 15
Q_2 = 25
T_1 = 12

# sigma points parameters
alpha = 0.1
beta = 2
kappa = 0
n = 8

# create sigma points and weights
points = MerweScaledSigmaPoints(n=8,alpha=.1,beta=2.,kappa=0)
sigmas = points.sigma_points(mean_zero,P_zero)

# sigma points weights
w_lambda = alpha**2 * (n + kappa) - n
Wc = np.full(2*n+1,0.5/(n + w_lambda))
Wm = np.full(2*n+1,0.5/(n + w_lambda))
Wc[0] = w_lambda / (n + w_lambda) + (1 - alpha**2 + beta)
Wm[0] = w_lambda / (n + w_lambda)

#process noise
Q = np.diagflat(((1*10**-5)*mean_zero)**2)
Q_n = np.random.multivariate_normal(np.zeros(8),Q,8)

# mesurement noise
R = (9*10**-2)*np.eye(2)
R_n = np.random.multivariate_normal(np.zeros(2),R,2)

#nonlinear_state_function
def f_nonlinear_state(T_2,T_3,R_21,R_32,C_2,C_3,w_1,w_2):
    T_2 = T_2 + dt*(-1*(T_2/C_2)*(1/R_21 + 1/R_32) + T_1/R_21*C_2 + T_3/R_32*C_2)
    T_3 = T_3+ dt*(-1*(T_3/C_3)*(1/R_32)+T_2/R_32*C_3 +w_1*Q_1/C_3 + w_2*Q_2/C_3)
    R_21 = R_21
    R_32 = R_32
    C_2 = C_2
    C_3 = C_3
    w_1 = w_1
    w_2 = w_2
    z = np.array([T_2,w_2])
    return z

# passing sigma points through nonlinear_state_function
sigmas_f = np.empty((17,8))
for i in range(17):
    sigmas_f[i] = f_nonlinear_state(sigmas[i,0],sigmas[i,1],2],3],4],5],6],7])

ukf_f_mean,ukf_f_cov = unscented_transform(sigmas_f,points.Wm,points.Wc)

# nonlinear mesurement function
def h_mesurement(T_2,T_3):
    T_2 = T_2
    T_3 = T_3
    y = np.array([T_2,T_3])
    return y
# passing sigmas_f through mesurement function 
sigmas_h = np.empty((17,2))
for i in range(17):
    sigmas_h[i] = h_mesurement(sigmas_f[i,sigmas_f[i,1])

ukf_h_mean,ukf_h_cov = unscented_transform(sigmas_h,points.Wc)

# cross covarinace
Pfh = np.zeros((8,2))
for i in range(17):
    Pfh += Wc[i] * np.outer(sigmas_f[i] - ukf_f_mean,sigmas_h[i] - ukf_h_mean)

K = np.dot(Pfh,np.linalg.inv(ukf_h_cov)) # Kalman gain

h = np.array([47.39642954,55.42371109]) # True value of the estimate 

New_mean = ukf_f_mean + np.dot(K,h-ukf_h_mean ) 
New_covarince = ukf_f_cov - np.dot(K,ukf_h_cov).dot(K.T)
New_covarince

解决方法

暂无找到可以解决该程序问题的有效方法，小编努力寻找整理中！

如果你已经找到好的解决方法，欢迎将解决方案带上本链接一起发送给小编。

小编邮箱:dio#foxmail.com (将#修改为@）