Уменьшите джиттер в живом фильтре Калмана - PullRequest
Новая
       103

Уменьшите джиттер в живом фильтре Калмана

0 голосов
/ 20 апреля 2020

У меня есть живой поток 1-D данных о местоположении с течением времени, который я подаю в фильтр Калмана. Как я могу точно настроить свой алгоритм для уменьшения дрожания пилообразного сигнала, который дает мне фильтр Калмана? Какие еще меры я могу предпринять для достижения более плавного результата?

enter image description here

data 

import numpy as np
import pandas as pd
from pykalman import KalmanFilter
from numpy import ma
import matplotlib.pyplot as plt

# Locate and clip relevant snippet of motion, removing unecessary data at the beginning and end
rearwheel_y = rearwheel_y[385: 600]

# The numpy.ma module provides a convenient way to address the issue of dropout, with masked arrays.
# When an element of the mask is False, the corresponding element of the associated array is valid and is said to be unmasked.
# When an element of the mask is True, the corresponding element of the associated array is said to be masked (invalid).
rearwheel_y = ma.masked_values(rearwheel_y, 0)

# time step
dt = 1
# initial_state_mean
X0 = 192.25871291 #rearwheel_y[0]
# initial_state_covariance
P0 = 0
n_timesteps = len(rearwheel_y)
n_dim_state = 1
filtered_state_means = np.zeros((n_timesteps, n_dim_state))
filtered_state_covariances = np.zeros((n_timesteps, n_dim_state, n_dim_state))
# Kalman-Filter initialization
kf = KalmanFilter()
# iterative estimation for each new measurement
for t in range(n_timesteps):
    if t == 0:
        filtered_state_means[t] = X0
        filtered_state_covariances[t] = P0
    elif t != 0:
        filtered_state_means[t], filtered_state_covariances[t] = (
        kf.filter_update(
            filtered_state_means[t-1],
            filtered_state_covariances[t-1],
            observation = rearwheel_y[t])
        )

# plot of the resulting trajectory
plt.figure()
plt.plot(rearwheel_y, 'k.', label = 'Observations')
plt.plot(filtered_state_means[:, 0], "g-", label="Filtered Positions", markersize=1)
plt.ylim(150, 205)
plt.grid()
handles, labels = plt.gca().get_legend_handles_labels()
by_label = dict(zip(labels, handles))
plt.legend(by_label.values(), by_label.keys())
...