balzer82 / kalman Goto Github PK
View Code? Open in Web Editor NEWSome Python Implementations of the Kalman Filter
Some Python Implementations of the Kalman Filter
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.
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.
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.
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.
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
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!
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'
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?
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
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
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
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
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, 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.
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.
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!
Hello,
There is a question: Why you CTRA Model Joccabi Matrix is the same with CTRV Model;
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?
A declarative, efficient, and flexible JavaScript library for building user interfaces.
🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
An Open Source Machine Learning Framework for Everyone
The Web framework for perfectionists with deadlines.
A PHP framework for web artisans
Bring data to life with SVG, Canvas and HTML. 📊📈🎉
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
Some thing interesting about web. New door for the world.
A server is a program made to process requests and deliver data to clients.
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
Some thing interesting about visualization, use data art
Some thing interesting about game, make everyone happy.
We are working to build community through open source technology. NB: members must have two-factor auth.
Open source projects and samples from Microsoft.
Google ❤️ Open Source for everyone.
Alibaba Open Source for everyone
Data-Driven Documents codes.
China tencent open source team.