Broadcasting error in EKF

[aayushsingla]

Hello Sir, I'm facing a broadcasting error in the first iteration, when estimating using EKF. I can't figure why this should happen. This is my code below:

EKF object creation:

                self.ekf = EKF(Dt = sampling_time,magnetic_ref=60.0)
        self.Q_last = acc2q(acc_vec)

I'm using following lines for updating:

        self.Q = self.ekf.update(self.Q_last, gyr=gyro_vec, acc= acc_vec,mag=mag_vec    
                self.Q_last = self.Q

The data of my parameter for 1st iteration: mag-data [ 32.20800018 -32.30559921 -27.05959892] acc-data [-0.58654064 3.85064983 8.75533772] gyro-data [ 0.05772506 -0.00748288 -0.01084254] Q-last [ 0.97815766 0.20559691 0.02996388 -0.00629804]

Error: ValueError: operands could not be broadcast together with shapes (6,6) (3,3) In line: S = H@P_t@H.T + self.R # Measurement Prediction Covariance

[aayushsingla]

Also, this code works if either of mag_vec or acc_vec is 0. @Mayitzin please let me know if you can reproduce this error or if this is mistake from my side.

[Mayitzin]

I can reproduce it. As soon as I fix this I'll commit it, so that it is better used. Thanks for reporting it!

[Mayitzin]

Fixed with bbf9674a921c3daf737136bc1366be2db4eb6044

The class EKF was defining a measurement noise covariance matrix at the very beginning of the creation of the object. This matrix, however, is defined as a 3x3 if only accelerometers are used or as a 6x6 if both accelerometers and magnetometers are used.

The problem came when, at the creation of the object, no magnetometer data was given, so it assumed it was only accelerometers, creating a 3x3 matrix. This matrix could not be used when the magnetometers were actually used.

Now it has been changed, so that the measurement matrix is re-defined every time the method update() is called. In this case, at the call of this method the user gives the used sensors, and based on this, the measurement noise covariance matrix is re-defined.

This also gives the possibility of using update() with or without magnetometers at any time during the estimation.

Let me know if this fixes it for you, so that we can close this issue. Thanks!

[aayushsingla]

Thanks @Mayitzin for looking at this. I'll let you know if this works for me..asap

[aayushsingla]

@Mayitzin I am still getting the same error. I initially thought it is fixed. But the error is still the same.

[aayushsingla]

@Mayitzin Okay, I got a bit of clarity on this issue here. your code fixes the issue here. However, the issue still persists if either of mag or acc is passed as zero in the update function. That's the reason my code was behaving a bit different. Code still works if you chose not to pass either of mag or acc vec but, the issue comes back if u pass them as zero.

[Mayitzin]

Ah ok, I think I understand. Indeed, a zero vector would quash the estimation, but mainly because the accelerometers and magnetometers are used to correct the estimation with the gravitational acceleration and Earth's magnetic field, respectively.

If the accelerometer measurements are all equal to zero, means there is no acting force holding your device against gravity (it is in free fall.) If the accelerometer reading is equal to zero in the EKF, it will not perform the update and simply return the previous orientation (quaternion.)

q_new = ekf.update(q=q_old, gyr=gyro_sample, acc=[0.0, 0.0, 0.0]) # This would return q_old
q_new = ekf.update(q=q_old, gyr=gyro_sample, acc=None) # This would fail

If the magnetometer readings are all equal to zero, it means your device is not inside the Magnetosphere (at least 65000 km above ground - basically far in space.) If you do not require these measurements, I would recommend not passing them, or make them equal to None. With it, the update will simply discard the magnetometer readings.

q_new = ekf.update(q=q_old, gyr=gyro_sample, acc=acc_sample, mag=[0.0, 0.0, 0.0]) # This would fail
q_new = ekf.update(q=q_old, gyr=gyro_sample, acc=acc_sample, mag=None) # This would correct without magnetometer
q_new = ekf.update(q=q_old, gyr=gyro_sample, mag=None) # This would complain that there is no accelerometer reading

I could add a handler to automate this "auto None" for the magnetometer, but first I would like to know if this is precisely what is not working properly. Did I get the problem right?

[aayushsingla]

Yes, you understood it right. adding a handler for the magnetometer should do.

[Mayitzin]

I think, for this case, the best solution is to add a warning, as trying to catch possible user errors and fixing them would start a series of patches that do not actually have to do with the algorithm.

If the user gives [0, 0, 0] for a magnetometer it must not try to fix it. It is better to warn the user, that its data is wrong, as it may be used in other instances somewhere else, without the user knowing it is not valid.

I'll comment again with the commit addressing this issue.

[Mayitzin]

Hi! Sorry for the late response. I added the Error Handling with the commit a48210e and now it ill raise whenever the user attempts to utilize an invalid geomagnetic field.