Python 3.x 卡尔曼滤波器基本应用/学习-代码似乎非常慢

我有一个旋转的平台和一个测量其位置的传感器。我试图编程一个简单的卡尔曼滤波器来平滑测量。我的测量运行长度大约在10000-15000个数据点之间。模拟时间超过3分钟

我希望代码会更快，因为kalman用于实时应用程序。另外，我用测量值做的其他计算几乎不需要那么长的时间，而且几乎是即时的。或者这是正常的，因为矩阵运算

我主要使用这个示例作为模板。下面是我的代码，也许有可能用不同的方式来编写，以加快速度

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

我找到了答案

在for迭代中为dt和Z调用pandas“address”使代码变得非常慢。我为dt和z数组创建了两个新变量，现在我的代码几乎是即时的。 帮助我认识到我的问题所在。对于任何读到这篇文章并有类似问题的人来说，问这样的问题也会是一个更好的地方

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