Kalman filter is now O(N^2) instead of O(N^3) with the number of planes. Like a total boss
Made P transformation (not tested) and verified F