卡尔曼滤波器基本应用/学习 - 代码似乎很慢

Posted 2023-02-21

【中文标题】卡尔曼滤波器基本应用/学习 - 代码似乎很慢

【英文标题】：Kalman filter basic application/learning - Code seems very slow

【发布时间】：2018-06-28 06:23:42

【问题描述】：

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

我预计代码会更快，因为卡尔曼用于实时应用程序。此外，我对测量所做的其他计算几乎不需要那么长时间，而且几乎是即时的。还是因为矩阵运算这是正常的？

我主要将此示例用作模板Kalman Filter Implementation for Constant Velocity Model (CV) in 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】：

我找到了答案。

在 dt 和 Z 的 for 迭代中调用 pandas 的“地址”会使代码非常慢。我为 dt 和 z 数组创建了两个新变量，现在我的代码几乎是即时的。 PythonSpeedPerformanceTips 帮助我意识到我的问题出在哪里。此外，StackEchange Code Review 对于阅读本文并遇到类似问题的任何人来说都是一个更好的地方。

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