Kalman фильтр базовое приложение / обучение - код кажется очень медленным - PullRequest
Новая
       24

Kalman фильтр базовое приложение / обучение - код кажется очень медленным

0 голосов
/ 28 июня 2018

У меня есть вращающаяся платформа и один датчик, измеряющий ее положение. Я попытался запрограммировать простой фильтр Калмана для сглаживания измерений. Мои измерения имеют длину около 10000-15000 точек данных. Симуляция длится более 3 минут.

Я ожидал, что код будет намного быстрее, поскольку kalman используется в приложениях реального времени. Также другие вычисления, которые я сделал с измерениями, не занимают так много времени и являются почти мгновенными. Или это нормально из-за матричных операций?

Я в основном использовал этот пример в качестве шаблона Реализация фильтра Калмана для модели постоянной скорости (CV) в Python . Ниже мой код, может быть, есть возможность написать его по-другому, чтобы сделать его быстрее? 

import numpy as np
import matplotlib.pyplot as plt

x = np.matrix([[0.0, 0.0]]).T # initial state vector x and x_point

P = np.diag([1.0, 1.0]) #!!! initial uncertainty 

H = np.matrix([[1.0, 0.0]]) # Measurement matrix H 

StD = 20 # Standard deviation of the sensor 

R = StD**2 # Measurment Noise Covariance 

sv = 2 # process noise basically through possible acceleration

I = np.eye(2) # Identity matrix

for n in range(len(df.loc[[name], ['Time Delta']])):
    # Time Update (Prediction)
    # ========================
    # Update A and Q with correct timesteps
    dt = float(df.loc[[name], ['Time Delta']].values[n])

    A = np.matrix([[1.0, dt],
                  [0.0, 1.0]])

    G = np.matrix([dt**2/2,dt]).T #!!! np.matrix([[dt**2/2], [dt]]) # Process Noise

    Q = G*G.T*sv**2    # Process Noise Covariance Q 

    # Project the state ahead
    x = A*x # not used + B*u
    # Project the error covariance ahead
    P = A*P*A.T + Q

    # Measurement Update (Correction)
    # ===============================
    # Compute the Kalman Gain
    S = H*P*H.T + R

    K = (P*H.T) * S**-1 #!!! Use np.linalg.pinv(S) instead of S**-1 if S is a matrix 

    # Update the estimate via z
    Z = df.loc[[name], ['HMC Az Relative']].values[n]
    y = Z - (H*x)                           
    x = x + (K*y)

    # Update the error covariance
    P = (I - (K*H))*P

1 Ответ

0 голосов
/ 29 июня 2018

Я нашел ответ.

Вызов панд "адресом" в итерации for для dt и Z сделал код очень медленным. Я создал две новые переменные для массивов dt и z, теперь мой код работает практически мгновенно. PythonSpeedPerformanceTips помогли мне понять, где моя проблема. Кроме того, Обзор кода StackEchange было бы лучшим местом для того, чтобы задать такой вопрос для тех, кто читает это и имеет похожую проблему. 

import numpy as np
import matplotlib.pyplot as plt

x = np.matrix([[0.0, 0.0]]).T # initial state vector x and x_point

P = np.diag([1.0, 1.0]) #!!! initial uncertainty 

H = np.matrix([[1.0, 0.0]]) # Measurement matrix H 

StD = 20 # Standard deviation of the sensor 

R = StD**2 # Measurment Noise Covariance 

sv = 2 # process noise basically through possible acceleration

I = np.eye(2) # Identity matrix

timesteps = df.loc[[name], ['Time Delta']].values
measurements = df.loc[[name], ['HMC Az Relative']].values

    for n in range(len(df.loc[[name], ['Time Delta']])):
    # Time Update (Prediction)
    # ========================
    # Update A and Q with correct timesteps
    dt = timesteps[n]

    A = np.matrix([[1.0, dt],
                  [0.0, 1.0]])

    G = np.matrix([dt**2/2,dt]).T #!!! np.matrix([[dt**2/2], [dt]]) # Process Noise

    Q = G*G.T*sv**2    # Process Noise Covariance Q 

    # Project the state ahead
    x = A*x # not used + B*u
    # Project the error covariance ahead
    P = A*P*A.T + Q

    # Measurement Update (Correction)
    # ===============================
    # Compute the Kalman Gain
    S = H*P*H.T + R

    K = (P*H.T) * S**-1 #!!! Use np.linalg.pinv(S) instead of S**-1 if S is a matrix 

    # Update the estimate via z
    Z = measurements[n]
    y = Z - (H*x)                           
    x = x + (K*y)

    # Update the error covariance
    P = (I - (K*H))*P
...