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

Question

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

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

我主要使用这个例子作为 Python 中恒速模型 (CV( 的卡尔曼滤波器实现模板。下面是我的代码，也许有可能以不同的方式编写它以使其更快？

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

Answer 1

我找到了答案。

在 dt 和 Z 的迭代中调用熊猫"地址"会使代码非常慢。我为 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