After you recalculate Rvv, in Rvv= np.array([[4*MDw[i]]]), don’t you then need to recalculate Ree = dot(C, dot(P[i], C.T)) + Rvv before using Ree to calculate the Kalman gain? Otherwise, the calculation of (and correction due to) the Mahalanobbis distance based on a measurement at time t will be applied to the measurement at time t+1. I’m not very familiar with the Mahalanobbis distance based outlier rejection, but this seems odd.
After you recalculate
Rvv, inRvv= np.array([[4*MDw[i]]]), don’t you then need to recalculateRee = dot(C, dot(P[i], C.T)) + Rvvbefore usingReeto calculate the Kalman gain? Otherwise, the calculation of (and correction due to) the Mahalanobbis distance based on a measurement at time t will be applied to the measurement at time t+1. I’m not very familiar with the Mahalanobbis distance based outlier rejection, but this seems odd.