Я лично смотрел код фильтра Калмана. Менял его для себя. Возможно ещё что-то понадобится