Added kalmanfilter support to AdvancedVisualAltimeter and RotationInvariantAltimeter. Corrected error in initialization of values of kalman_filter.measurementMatrix.