Update Sensor Filtering Algorithms with Modified Implementation

[cobbleobelisk]

Current sensor filter algorithm implementation is causing incorrect filtering logic. Once new approach/algorithms (if needed) is defined through research, new filtering logic must be implemented in place of existing, flawed logic.

[cobbleobelisk]

Through some testing, I believe implementing process noise consideration into the sensor filtering algorithm is neccessary, specifcially using a higher process noise so that new measurements have a larger effect on predicted state for that measurement reading (as this filtering should only be to smooth data recieved from raw sensors to remove various biases, before actual sensor fusion can be applied). Over this week i will determine exact numbers to use for process noise for each sensor and implement modified approach.