Testing existing libraries for matrix operations and Kalman filter

MateoMiguelez commented 10 months ago

The aim is to research and test possible libraries starting with http://strawsondesign.com/docs/librobotcontrol/ which is very well documented and should have everything we need

MateoMiguelez commented 7 months ago

Kalman filter is working. Here is an example of 1 set of K [1,0.3288], 4 sensors and the following amplitudes [0,0,0,1,0,0,0,0]. There is random noise for both, process and measurements added into the system

MateoMiguelez commented 7 months ago

Next, we want to know how the system will behave given a k(k1,k2) that gives the same temporal frquency as our real example but has different values k1,k2 (Same magnitude different components). To test this, we have created different scenarios: Scenario 1 Wave simulation with k = [0.926342,0.5], 4 sensors placed in [1,1],[1,2],[2,1],[2,2] and the following amplitudes a = [0,0,0,1,0,0,0,0] Kalman filter with k =[0.926342,0.5] and we get the following results

Scenario 2 Wave simulation with k = [1,0.3288], 4 sensors placed in [1,1],[1,2],[2,1],[2,2] and the following amplitudes a = [0,0,0,1,0,0,0,0] Kalman filter with k =[0.926342,0.5], which differs in 9.3 degrees with the simulated K .

Scenario 3 Wave simulation with k = [1,0.3288], 4 sensors placed in [1,1],[1,2],[2,1],[2,2] and the following amplitudes a = [0,0,0,1,0,0,0,0] Kalman filter with k =[0.7426,0.7426], which differs in about 30 degrees with the simulated K

Scenario 4 Wave simulation with k = [1,0.3288], 4 sensors placed in [1,1],[1,2],[2,1],[2,2] and the following amplitudes a = [0,0,0,1,0,0,0,0] Kalman filter with k =[-0.3288,1], which differs in 90 degrees with the simulated K

Scenario 4 Wave simulation with k = [1,0.3288], 4 sensors placed in [1,1],[1,2],[2,1],[2,2] and the following amplitudes a = [0,0,0,1,0,0,0,0] Kalman filter with k =[0.3288,1], which differs in 59.3 degrees with the simulated K

Scenario 5 Wave simulation with k = [1,0.3288], 4 sensors placed in [1,1],[1,2],[2,1],[2,2] and the following amplitudes a = [0,0,0,1,0,0,0,0] Kalman filter with k =[0.6866,0.7944], which differs in 5 degrees with the simulated K

Scenario 6 Exactly same configuration as Scenario 2 but with different sensor positioning. 4 sensors placed in [1,1],[2,2],[4,1],[3,2]. As we can see, this has a great impact on performance

MateoMiguelez commented 7 months ago

We also want to test how we could use a grid of wavenumbers (K) and try to work as best as possible with an infinite number of frequencies. Scenario 1 We will start easy by having a 1 K simulation and checking in our kalman filter for 3 different wavenumbers, one of them being the exact simulated K. That is K = [0.707,0.707] for our simulation with amplitudes a = [0,0,0,0.5,0,0.5,0,0,]. In our Kalman filter we have: K = [0.707, 0.707 ; 45,107 ; 0.222,-0.056 ]. We have chosen K values that are far apart from each other to try to make it as easy as possible. We got the following results

The first image refers to the first K vector. This is the one that should match exactly with our simulation, The 2nd and 3rd screenshots show the amplitudes calculated by the filter for the second vector K, which should be as close to zero as possible

MateoMiguelez commented 6 months ago

We now want to create a grid with different K values and check how the filter distributes a single unknown simulated k between near by points in the grid. We have generated K values for the filter with the script and start_k = 0.8 stop_k = 2 start_angle = 10 stop_angle = 20 Scenario 1 To start off easy, lets begin with a 4 vector grid and simulate exactly one of the Ks in this grid. The results are as follows

These are the results for each vector in the grid. The simulated K was EXACTLY the same as the first k in our grid and it still split the amplitudes among the first 2 vectors, which is not what I expected and far from ideal. However, if we plot the resulting eta for the kalman filter, it's quite accurate. Below the first image shows the eta for one of the for sensors, green is the filter and red is the simulation. After 10 seconds we "turn off" the sensors to check how accurately the filter is following reality. Zooming in we can see that while the sensor is on the filter is spot on predicting the eta (next image). There is 1 measurement every 25 updates.

Scenario 2 We will now introduce a random K that lays somewhere inside our grid and check how the filter distributes its amplitudes along the different vectors in our grid. In this case our random K is module 1.13 and angle 73.6 with amplitudes a3 and a5 = 2

This sort of oscillatory behaviour is worrying to me. But let's check the whole eta:

This results are quite bad. Scenario 3 Let's try to make our grid more dense with 16 points with the exact same upper and lower limits. We will use the exact same random k as before. I will not provide screenshots of every amplitude as there are too many. They are similar to the last scenario with some oscillations and some other stable values with smaller overall amplitude. Regarding the eta:

This is much better although the simulation has taken much longer, this is also something to have in mind as we need to run the filter in real time. Scenario 4 Lets create a big mesh that covers the whole range we want to cover for |k| and angles 0 to 90 start_k = 0.8 stop_k = 21 start_angle = 5 stop_angle = 80 To try to create a very dense mesh like the one in the previous scenario we would need over 60 modules and 28 angles which are too many. Lets start with 10 angles and 10 modules and check wether 100 points is a reasonable amount. The random K in our simulation has module 9.79 and angle 12.2 degrees Again, there is no point on plotting the amplitudes as there are way too many. Let's check the total eta:

Results are not bad, although simulation took way too long (and didn't finish, I only simulated 8 out of 20 seconds)

Lets try with a smaller mesh.

Scenario 5 Lets use a 25 point mesh, that is 5 angles and 5 modules.

These results are not great and the simulation still took over 25 minutes. Although we do need to keep in mind that the timestep in these simulations is 0.001 which is really small. The reason behind such a small timestep is to visualize how the filter is behaving in between measurements

MateoMiguelez commented 6 months ago

Execution time can be an issue. For a 25 point grid, we have recorded the time it takes for both prediction and update step. Here are some cycles and their corresponding times:

Time per iteration predict: 0.116000 Time per iteration predict: 0.005000

Time per iteration predict: 0.086000 Time per iteration predict: 0.007000

Time per iteration predict: 0.101000 Time per iteration predict: 0.009000

Time per iteration predict: 0.192000 Time per iteration predict: 0.007000

Time per iteration predict: 0.128000 Time per iteration predict: 0.007000

Time per iteration predict: 0.237000 Time per iteration predict: 0.003000

In the prediction step, the most time consuming isntruction is by far: if(rc_kalman_predict_simple(&kfilter, F) == -1) return -1; So we need to look how to optimize this as it takes way too long. We would not be able to run this 25 step grid filter in real time, as it needs to run at 0.025 seconds per iteration Inside the Kalman predict function call what takes up most of the time are matrix multiplications

MateoMiguelez commented 6 months ago

Malla de 4 con modulo entre 0.8 y 5. La simulacion tiene modulo 1.8

Malla de 16

Malla de 4 angulos con 1 solo modulo. En este caso se simula exactamente 1 par de frecuencias del filtro

Esta es la amplitud de la simulación y la resultante del filtro que deberían coincidir

MateoMiguelez commented 6 months ago

In order to be able to distinguish waves with same frquency coming from different directions we are going to try: Adding a moving boat and moving the sensors as a whole. Simulation with 4 point mesh of 1 freq and multiple angles, just like the last picture in the last comment. Here the boat is moving at about 5m/s with 45º angle and wave has a direction of about 50 degrees

Now the boat is moving at 5m/s but at a 90º angle

We now move the boat faster (15m/s) in 45º degree angle with same 50º wave

And as we can check, moving at a 90º course does not improve

With this in mind lets try to move sensors in a block from side to side in a sinusoidal way. So we will now move forward in the y direction and have the sensors move as sin(2pi10000t) in the x direction

Let's try with a slower freq, as 10KHz would be way too fast anyway we'll try with 100Hz

Let's try moving the sensors in a wider motion, again, at 10KHz

MateoMiguelez commented 5 months ago

Depués de debuggear, parece que ya las cosas funcionan como deberían. A continuación quiero mostrar la diferencia entre 4 y 9 sensores, que es una de las razones que nos llevó a pensar que antes no estaba funcionando bien. Quiero destacar que con 4 y 9 sensores, el error en amplitud es similar, pero sin embargo, la velocidad es mucho más precisa con más sensores. 4 Sensores Comenzamos con un gráfico mostrando la diferencia en amplitud entre la simulación y el filtro con 4 sensores. Son las 8 amplitudes de la primera K del filtro, que deberían coincidir con la K de la simulación, puesto que son iguales.

A continuación vemos la diferencia en la velocidad del agua en el eje x calculada para la simulación y para el filtro con el sistema que usa 4 sensores

9 Sensores Se muestra la diferencia en amplitudes para el caso de los 9 sensores, el resto de parámetros permanecen iguales. Es notable que la diferencia, especialmente en algunas amplitudes sigue siendo grande.

Sin embargo, como vemos en la imagen inferior, la velocidad es mucho más precisa

MateoMiguelez commented 5 months ago

Prueba con una malla con muchos módulos (10 módulos), todos con el mismo ángulo. Simulación de una K que no está en la malla. Y solo 2 sensores: Representación de las velocidades: