First set of changes to Kalman Filter calculation

Made changes to fix matrix addition issues.