Broadcasting error in EKF

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

[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]

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?