我想实现无迹卡尔曼滤波器（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 , T_3,R_21 , R_32,C_2 ,C_3 ,w_1,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],sigmas[i, 2],sigmas[i, 3],sigmas[i, 4],sigmas[i, 5],sigmas[i, 6],sigmas[i, 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, 0], sigmas_f[i ,1])
ukf_h_mean, ukf_h_cov = unscented_transform(sigmas_h, points.Wm, 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
这是 UKF 的一个已知问题。求矩阵的平方根是一项具有挑战性的任务。使用 Cholesky 分解，我们可以计算矩阵的平方根。但它有一个条件。矩阵需要是半正定 (SPD)。通常，由于计算过程中非常小的错误，矩阵会偏离 SPD。当这种情况发生时，相信我，这种情况经常发生，您需要计算“最近的”SPD。
这是一种可以提供帮助的新颖方法：计算最近的对称正半定矩阵以及来自 mathworks 的示例代码：nearestSPD
nearestSPD 函数将矩阵作为输入（在您的情况下，这将是协方差矩阵）并输出该矩阵的最近的半正定。
这是 Masmm 描述的 UKF 的典型问题。我还找到了近 PSD 算法的实现here。然而，没有一个答案可以完美解决问题。
您可以逐步遵循三个步骤：
最棘手的是，前两步并不能保证矩阵可以被 Cholesky 处理。所以我的解决办法是循环添加抖动，直到满意为止。还有其他方法，本文paper一共回顾了6种方法。
def get_near_psd(P, max_iter=10):
eps = 1e-3 # Small positive jitter for regularization
increment_factor = 10 # Factor to increase eps if needed
def is_symmetric(A):
return np.allclose(A, A.T)
def is_positive_definite(A):
try:
np.linalg.cholesky(A)
return True
except np.linalg.LinAlgError:
return False
for _ in range(max_iter):
if is_symmetric(P) and is_positive_definite(P):
return P # The matrix is suitable for Cholesky
# Make P symmetric
P = (P + P.T) / 2
# Set negative eigenvalues to zero
eigval, eigvec = np.linalg.eig(P)
eigval[eigval < 0] = 0
# add a jitter for strictly positive
eigval += eps
# Reconstruct the matrix
P = eigvec.dot(np.diag(eigval)).dot(eigvec.T)
# Check if P is now positive definite
if is_positive_definite(P):
return P
# Increase regularization factor for the next iteration
eps *= increment_factor
raise ValueError("Unable to convert the matrix to positive definite within max iterations.")