How to create Kalman Filter for 3d object tracking

Question

I need to create Kalman Filter for 3d object tracking in python I don’t understand how should I create these matrices and from where take the measurements If there are any examples of KD for 3d object just share with me please

i have seen KD for 2d object tracking But there already were all the matrices given And the only thing I had to do is to write that prediction and update functions

A = np.array([[1, 0, 0, dt, 0, 0],
              [0, 1, 0, 0, dt, 0],
              [0, 0, 1, 0, 0, dt],
              [0, 0, 0, 1, 0, 0],
              [0, 0, 0, 0, 1, 0],
              [0, 0, 0, 0, 0, 1]])

# Noise covariance matrix
Q = q**2 * np.array([[dt**3/3, 0, 0, dt**2/2, 0, 0],
                     [0, dt**3/3, 0, 0, dt**2/2, 0],
                     [0, 0, dt**3/3, 0, 0, dt**2/2],
                     [dt**2/2, 0, 0, dt, 0, 0],
                     [0, dt**2/2, 0, 0, dt, 0],
                     [0, 0, dt**2/2, 0, 0, dt]])

# Measurement matrix
H = np.array([[1., 0, 0, 0, 0, 0],            
              [0., 1, 0, 0, 0, 0],
              [0., 0, 1, 0, 0, 0]])
# Measurement noise covariance matrix
R = 5 * np.eye(3)

that's how my matrices look now but I think the matrix Q is not correct I don't understand how should I create it

Welcome to SO, HowTo questions are generally frowned upon on this platform. Try sharing what you have tried already and what you would like to accomplish specifically. For example: If you have code for 2d object tracking, you could include it in your question and then ask how to extend the code for 3d tracking. — Alex V., Apr 04 '23 at 08:12

Alexey R. · Answer 1 · 2023-04-04T16:34:32.140

No matter how many dimensions you have. Kalman Filter is not about positions but about general solution of how to predict a system state.

In general you need to have state transition matrix that is in case of movement normally adds velocity columns - one for each dimension - since when moving an object adds certain amount of position each iteration.

You also need the covariance matrix that defines the covariance of state vector elements.

And also different matrices that adds certain amount of external input (like noise, control, etc.).

However in the simplest cases you do not need to set up all of them. Below is the simple example of 3D case with the help of Kalman Filter implementation of filterpy package.

import numpy as np
from filterpy.kalman import KalmanFilter

f = KalmanFilter(6, 3)

f.x = np.array([0., 0., 0., 0., 0., 0.])
f.F = np.asarray(
    [
        [1., 0., 0., 1., 0., 0.],
        [0., 1., 0., 0., 1., 0.],
        [0., 0., 1., 0., 0., 1.],
        [0., 0., 0., 1., 0., 0.],
        [0., 0., 0., 0., 1., 0.],
        [0., 0., 0., 0., 0., 1.]
    ]
)

f.H = np.array([
    [1., 0., 0., 0., 0., 0.],
    [0., 1., 0., 0., 0., 0.],
    [0., 0., 1., 0., 0., 0.]
])

def main():
    for i in range(10):
        z = np.array([i, 0.1*i, 0.01*i])
        print('Measured: ', z)
        f.predict()
        print('predicted: ', f.x)
        f.update(z)

if __name__ == '__main__':
    main()

Where

f.x is your initial state vector of [x, y, z, vx, vy, vz] where v means velocity.
f.F is your state transition matrix, i.e. the model describing the way you expect the position of your object would change. For example according to the model from example, your x would change as 1*x + 0*y + 0*z + 1*vx + 0*vy + 0*vz = x + vx each step.
f.H - The matrix that defines measurement function

The above example uses default covariance matrix, default noises, etc.

P.S. - Regarding from where take the measurements we cannot advice you anything. That depends on what data or what sensors you have.

thank you for your reply but the thing is that I don't have to use the libraries — user21562998, Apr 05 '23 at 22:47
You can just check how it is implemented. That's pure python library. Now I am not sure I understand your question. If your are asking how to implement Kalman Filter then it's too general question. — Alexey R., Apr 06 '23 at 12:18
so my task is to create a 3d state model simulation and kalman filter for it with constant velocity model but I don't understand how to create this simulation (do I have to use the KF for it or just use generating data) and regarding this kalman filter I don't understand what kind of Q matrix it will when it's constant velocity model in 3d if I understood correctly we construct this Q matrix based on G vector and Qa but I don't understant where this G vector coming from — user21562998, Apr 06 '23 at 12:48

How to create Kalman Filter for 3d object tracking

1 Answers1