Kalman Filtering in Python

Question

I've been trying to work on designing a Kalman Filter for a few weeks now, but I'm pretty sure I'm making a major error because my results are terrible. My common sense tells me it's because I'm using an already-existing matrix as my predicted state instead of using a transition matrix, but I'm not sure how to solve that if it indeed is the issue. By the way, this is my first time using Kalman Filtering, so I may be missing basic stuff.

Here is a detailed explanation:

I have 2 datasets of 81036 observations each, with each observation including 6 datapoints (i.e., I end up with 2 matrices of shape 81036 x 6). The first dataset is the measured state and the other one is the predicted state. I want to end up with a Python code that filters the data using both states, and I need the final covariance and error estimates. Here's the main part of my code:

    import numpy as np
    #nb of observations
    nn=81036
    #nb of datapoints
    ns=6

    #import
    ps=np.genfromtxt('.......csv', delimiter=',')
    ms=np.genfromtxt('.......csv', delimiter=',')

    ##kalman filtering with covariance
    #initialize data (lazy initialization using means of columns)
    xi=np.mean(ms,axis=0)

    for i in np.arange(nn):
        #errors
        d=ms[i,:]-xi
        d2=ps[i,:]-xi

        #covariance matrices
        P=np.zeros((ns,ns))
        R=np.zeros((ns,ns))
        for j in np.arange(ns):
            for s in np.arange(ns):
                P[j,s]=d[j]*d[s]
                R[j,s]=d2[j]*d2[s]

        #Gain
        k=P*(P+R)**-1
        #Update estimate
        xi=xi+np.matmul(k,d2)

        #Uncertainty/error
        I=np.identity(ns)
        mlt=np.matmul((I-k),P)
        mlt=np.matmul(mlt,((I-k).T))
        mlt2=np.matmul(k,R)
        mlt2=np.matmul(mlt2,k.T)
        Er=mlt+mlt2

When I run this code, I end up with my filtered state xi going through the roof, so I'm pretty sure this is not the correct code. I've tried to fix it in several ways (e.g., I tried to calculate the covariance matrix in the standard way I'm used to - D'D/n -, I tried to remove my predicted state matrix and simply add random noise to my measured state instead...), but nothing seems to work. I also tried some available libraries for Kalman Filtering (as well as libraries in Matlab and R), but they either work in 1D only or need me to specify variables like the transitional matrix, which I don't have. I'm at the end of my wits here, so I'd appreciate any help.

Answer 1

I've found the solution to this issue. Huge props to Kani for their comment, as it pointed me in the right direction.

It turns out that the issue is simply in the calculation of k. Although the equation is correct, the inverse function was not working properly because of the very small values in some instances of R and P. To solve this, I used the pseudoinverse instead, so the line for calculating k became as follows:

k = P @ np.linalg.pinv(P + R)

Note that this might not be as accurate as the inverse in other cases, but it does the trick here.