Negative covariance matrix in Unscented Kalman filter

Question

I want to implement the Unscented Kalman filter(UKF) method for the nonlinear problem; I set all initial values such as initial mean vector and initial covariance matrix. The dimension of the problem is 8. Below is the code of implementation; I do not know why the new covariance matrix at the end of the code includes some negative parameters. These negative parameters make problems for the next iteration of the approach.

#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

Answer 1

This is a typical problem for UKF as described by Masmm. I also found an implementation of the near PSD algorithm here. However, none of the answers solve the problem perfectly.

There are three steps you can follow step by step:

• make any matrix symmetric by P = (P + P.T) / 2.
• make it positive definite by replacing all negative eigenvalues with zero.
• Adding a jitter.

The trickiest thing is that the first two steps do not guarantee the matrix can be processed by Cholesky. So my solution is to add a jitter in a loop until it is satisfied. There are also other methods, 6 methods in total have been reviewed in this paper.

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.")

Answer 2

It is a known issue for UKF. Taking square root of a matrix is a challenging task. Using Cholesky factorization we can calculate square root of a matrix. But it has one condition. Matrix needs to be semi positive definite (SPD). Often, due to very small errors in computation process, matrices deviate from being SPD. When this happens and trust me it happens all the time, you need to calculate "nearest" SPD.

Here is a novel method that can help: Computing a nearest symmetric positive semidefinite matrix and sample code from mathworks: nearestSPD

nearestSPD function takes matrix as an input (in your case that would be the covariance matrix) and outputs the nearest semi positive definite of that matrix.