balzer82 / kalman Goto Github PK

Hello,

Thank you for putting these up. I'd like to know if your GPS position is already transformed to the COM of the robot? If not, how would we go about accounting for the offset in the position of the GPS in the data?

Thanks!

I am trying to employ the Kalman filter in project for object tracking in case of occlusion. Your implementation is very good and seems it will work for me, however I don't get the significance of variable 'm' and how will it change after every frame. I am not clear about its use. Can you clarify for me . Thanks.

I am sorry to ask a really simple question for I am Novice. Who can give me some information to understand this picuture about the 3d kalman, thanks!

Hallo,

wollte dir erst auf Twitter schreiben aber irgendwie kann ich da keine Direktnachricht schreiben. Auch wenn dein Code schon ein paar Jahre alt ist, hätte ich ein paar Fragen zu deinen Kalman-Filter Implementationen.
Erstmal vielen Dank dass du das veröffentlicht und so gut dokumentiert hast!
Ich muss selber einen "extended Kalman-Filter" entwerfen, der am besten Messungen aus GPS und IMU + Odometry vereint. Ich scheitere leider schon am aufstellen des "measurement vector", da meine GPS und IMU Daten unterschiedlich "lang" sind. Sprich die Sample Rate ist bei den IMU Daten höher (ist ja eigentlich immer so). Wie hast du es geschafft in deinen Testdaten die so schön nebeneinander zu reihen und was kann man machen wenn das nicht der Fall ist.
Hoffe du kannst mir das beantworten.
Gruß

Hi, I am learning kalman filter from your notebooks. I have a question for the notebook Kalman-Filter-CA-Ball. The covariance matrix Q in the notebook is something like this:

[[  2.50e-09   2.50e-09   2.50e-09   5.00e-07   5.00e-07   5.00e-07   5.00e-05   5.00e-05   5.00e-05]
 [  2.50e-09   2.50e-09   2.50e-09   5.00e-07   5.00e-07   5.00e-07   5.00e-05   5.00e-05   5.00e-05]
 [  2.50e-09   2.50e-09   2.50e-09   5.00e-07   5.00e-07   5.00e-07   5.00e-05   5.00e-05   5.00e-05]
 [  5.00e-07   5.00e-07   5.00e-07   1.00e-04   1.00e-04   1.00e-04   1.00e-02   1.00e-02   1.00e-02]
 [  5.00e-07   5.00e-07   5.00e-07   1.00e-04   1.00e-04   1.00e-04   1.00e-02   1.00e-02   1.00e-02]
 [  5.00e-07   5.00e-07   5.00e-07   1.00e-04   1.00e-04   1.00e-04   1.00e-02   1.00e-02   1.00e-02]
 [  5.00e-05   5.00e-05   5.00e-05   1.00e-02   1.00e-02   1.00e-02   1.00e+00   1.00e+00   1.00e+00]
 [  5.00e-05   5.00e-05   5.00e-05   1.00e-02   1.00e-02   1.00e-02   1.00e+00   1.00e+00   1.00e+00]
 [  5.00e-05   5.00e-05   5.00e-05   1.00e-02   1.00e-02   1.00e-02   1.00e+00   1.00e+00   1.00e+00]]

since it's a covariance matrix, and the value between all the acceleration are 1.0, I think this means acc_x and acc_y and acc_z are not Independent. For example, the following code make some random values with an all one covariance matrix:

import numpy as np
np.random.multivariate_normal([0, 0, 0], np.ones((3, 3)), size=5)

the output is:

array([[-0.8903466 , -0.8903466 , -0.8903466 ],
       [-0.47167442, -0.47167442, -0.47167442],
       [ 0.54420125,  0.54420125,  0.54420125],
       [-0.41152627, -0.41152627, -0.41152627],
       [-1.10581353, -1.10581353, -1.10581353]])

All the columns are the same value.

I think x, v_x, acc_x are dependent, y, v_y, acc_y are dependent, and z, v_z, acc_z are dependent, all other pairs are Independent. So the matrix Q is something like this:

[[  2.5e-11         0         0     5e-09         0         0     5e-07         0         0]
 [        0   2.5e-11         0         0     5e-09         0         0     5e-07         0]
 [        0         0   2.5e-11         0         0     5e-09         0         0     5e-07]
 [    5e-09         0         0     1e-06         0         0    0.0001         0         0]
 [        0     5e-09         0         0     1e-06         0         0    0.0001         0]
 [        0         0     5e-09         0         0     1e-06         0         0    0.0001]
 [    5e-07         0         0    0.0001         0         0      0.01         0         0]
 [        0     5e-07         0         0    0.0001         0         0      0.01         0]
 [        0         0     5e-07         0         0    0.0001         0         0      0.01]]

I am not sure I am right or not, just create this issue to discuss about this.

Hi Author,

Currently Im tracking the object with constant velocity model.
The parameter "dt" is time interval as we know in kalman filer.

My query is how to compute or choose optimal value of dt??
Im detecting object using computer vision algorithm at frame rate of 20FPS...
So I have computed dt = 20/30 (MyAlgo_FPS/CameraCapture_FPS).
Is my computation is right ?

I eagerly wait for prompt response.

Thanks,
Anil.

Hi, isn't it the correction step is followed after prediction?

So, for the prediction step if you use t+1 then same should be used. Please let me know if I'm missing something.

Hello Balzer,
Thank you very much for sharing the data and tutorial.

Are the omega values observed by the z-axis, termed as the Yawrate from imu ??
Or I am understanding it wrongly.

Thank you very much in advance.
Best regards,

Why are the transitions of the CTRA model x -> x + v/dpsi * () and not x -> x + v * sin (psi) ?
Where does the v/dpsi term come from?

Also, I'd like to add changing acceleration and turning.
Do you have a good source for these equations?

Thanks!
Ido

Hello all!!

I am working on comparing some movement models for an ackerman steering car. I used the bicycle model and now the CTRA. Both work quite good (still finishing my comparison), but I am stuck right now wondering something:

In the bicycle model I use Linear Speed and Steer Angle as CONTROL inputs inside my moveModel function. So as an example, the speed on instant t1 is dependent only of the control input of Speed on instant t1, it's not dependant of the Speed State of the system on time t0.

In the CTRA I see that the Linear Speed is considered a MEASUREMENT, not a control input. This is fine, because actually what I can control is linear acceleration, not linear speed. But wouldn't it be better if, instead of defining the acceleration as constant (in the CTRA jupyter notebook this is in the command x[5] = x[5]), we simply define acceleration at instant t1 equal to the CONTROL input of the acceleration that I am applying to the system at instant t1?

I will actually run this and try to post the result here. If I am wrong I can correct myself, or maybe continue the discussion!

Thank you so much!

PS: I am using UKF instead of EKF but I don't think that this is relevant for the question stated above.

Hello,
There is a question: Why you CTRA Model Joccabi Matrix is the same with CTRV Model;

Hi,

I am trying to use your CTRV code to calculate an accurate course of a car to compare to a heading obtained from an imu that has sensor fusion on board in order to calculate slip angle for a car drifting application. I just have a few questions as I can't seem to get it to work correctly.

In your raw data file "2014-03-26-000-Data.csv" are the units for yawrate degrees per second, positive for clockwise and negative for anti clockwise? and are the units for ax m/s^2, positive for acceleration and negative for deceleration?

Also can you explain why you offset the course "course = (-course + 90.0)"?

Thanks

Thank you for the tutorial and notebooks!

In Extended-Kalman-Filter-CHCV you reference some constants, but not all of them are clear to me.

    sGPS     = 0.5*8.8*dt[filterstep]**2  # assume 8.8m/s2 as maximum acceleration
    sCourse  = 2.0*dt[filterstep] # assume 0.5rad/s as maximum turn rate
    sVelocity= 35.0*dt[filterstep] # assume 8.8m/s2 as maximum acceleration

• 8.8 m/s2 seems reasonable; one site comparing various race cars suggests Formula 1 accelerates at upwards of 11.9 m/s2 (90kmph in 2.1sec), but the other two (Indy and NASCAR) are 8.3 and below;
• 0.5 rad/s seems decent, I'm not certain of its perfectness (for the same cars), are the consequences of over-estimating turn rate significant? (Is it "just" under-filtering or lack of sensitivity to outliers?)
• 2.0?
• 35.0?

ValueError Traceback (most recent call last)
Cell In[20], line 18
16 a23 = dt[filterstep]*x[3]np.cos(x[2])
17 a24 = dt[filterstep]np.sin(x[2])
---> 18 JA = np.matrix([[1.0, 0.0, a13, a14],
19 [0.0, 1.0, a23, a24],
20 [0.0, 0.0, 1.0, 0.0],
21 [0.0, 0.0, 0.0, 1.0]])
24 # Calculate the Process Noise Covariance Matrix
25 sGPS = 0.58.8dt[filterstep]**2 # assume 8.8m/s2 as maximum acceleration

File ~\anaconda3\envs\drone\Lib\site-packages\numpy\matrixlib\defmatrix.py:146, in matrix.new(subtype, data, dtype, copy)
143 data = _convert_from_string(data)
145 # now convert data to an array
--> 146 arr = N.array(data, dtype=dtype, copy=copy)
147 ndim = arr.ndim
148 shape = arr.shape

ValueError: setting an array element with a sequence. The requested array has an inhomogeneous shape after 2 dimensions. The detected shape was (4, 4) + inhomogeneous part.

You everywhere divide speed of car by 3.6. What the magic number? And why we need this? Strange, but without this doesn't work properly

By executing the line 'K = (P*JH.T) * np.linalg.inv(S)' I get always an error:

The original S is matrix([[matrix([[6371.]]), matrix([[6371.]])],
[matrix([[6371.]]), matrix([[6371.]])]], dtype=object)

TypeError: No loop matching the specified signature and casting was found for ufunc inv

if I convert s to float64 S = S.astype(np.float64) the content of S is

matrix([[6371., 6371.],
[6371., 6371.]])

LinAlgError: Singular matrix

How can I solve this issue?

I read the notebooks and there I see an actor G is used to calculate Matrix Q every time. I am unable to understand whats the role of G (Actor) can you please point me towards some resources which might explain in detail how we come up with this formulation ?
Thanks alot

Hi, this is an amazing content. I would like to filter my 2D signal to obtain both the velocity and acceleration at a time t, in order to estimate a CTRA model. Which method would you recommend to me? I am using the Argoverse Motion Forecasting dataset.

An example of what I am trying to do is as following:

Do you have a reference or code?

My name is Kritika Sharma and I am in the middle of making a project where I have to implement Kalman Filter. Now I have GPS long,lat coordinates(which I will be getting through android phone) and I want to predict the velocity of the car the phone is in. Can you tell me its code. How can we implement it? What and how will we input our data and get our output? My email id is [email protected]. It is urgent and if you could help me it'll be great.

Hi, for some reason I can't read the notebook for your process here. Would you be able to reupload it or something? Thank you for your time!

different lengths or shapes) is deprecated. If you meant to do this, you must specify 'dtype=object' when creating the ndarray.
arr = N.array(data, dtype=dtype, copy=copy)

UFuncTypeError Traceback (most recent call last)
/home/roy/softwares/Kalman/Extended-Kalman-Filter-CHCV.ipynb Cell 39' in <cell line: 1>()
47 S = JH @ P @ JH.T + R
48 S.astype('float64')
---> 49 K = (P @ JH.T) @ np.linalg.inv(S)
50 # K=(PJH.T)inv(JHPJH.T + R)#Kalman Gain
51 # Update the estimate via
52 Z = measurements[:,filterstep].reshape(JH.shape[0],1)

File <array_function internals>:180, in inv(*args, **kwargs)

File ~/.local/lib/python3.10/site-packages/numpy/linalg/linalg.py:545, in inv(a)
543 signature = 'D->D' if isComplexType(t) else 'd->d'
544 extobj = get_linalg_error_extobj(_raise_linalgerror_singular)
--> 545 ainv = _umath_linalg.inv(a, signature=signature, extobj=extobj)
546 return wrap(ainv.astype(result_t, copy=False))

UFuncTypeError: Cannot cast ufunc 'inv' input from dtype('O') to dtype('float64') with casting rule 'same_kind'