python卡尔曼多维_卡尔曼二维过滤器在python

这是基于

equations given on wikipedia的卡尔曼滤波器的实现。请注意，我对卡尔曼滤波器的理解非常简单，所以有可能的方法来改进这个代码。 (例如，它受到

here讨论的数值不稳定性问题的影响。据我所知，这只会影响Q值运动噪声非常小时的数值稳定性，在现实生活中，噪声通常不是很小，幸运的是至少对于我的实施)在实践中数值不稳定性不会出现。)

在下面的例子中，kalman_xy假定状态向量是4元组：位置的2个数字和速度的2个数字。

已经为该状态向量定义了F和H矩阵：如果x是4元组状态，则

new_x = F * x

position = H * x

然后调用卡尔曼，这是广义卡尔曼滤波器。一般来说，如果你想定义一个不同的状态向量 – 也许是代表位置，速度和加速度的6元组，它仍然是有用的。您只需通过提供适当的F和H来定义运动方程。

import numpy as np

import matplotlib.pyplot as plt

def kalman_xy(x, P, measurement, R,

motion = np.matrix('0. 0. 0. 0.').T,

Q = np.matrix(np.eye(4))):

"""

Parameters:

x: initial state 4-tuple of location and velocity: (x0, x1, x0_dot, x1_dot)

P: initial uncertainty convariance matrix

measurement: observed position

R: measurement noise

motion: external motion added to state vector x

Q: motion noise (same shape as P)

"""

return kalman(x, P, measurement, R, motion, Q,

F = np.matrix('''

1. 0. 1. 0.;

0. 1. 0. 1.;

0. 0. 1. 0.;

0. 0. 0. 1.

'''),

H = np.matrix('''

1. 0. 0. 0.;

0. 1. 0. 0.'''))

def kalman(x, P, measurement, R, motion, Q, F, H):

'''

Parameters:

x: initial state

P: initial uncertainty convariance matrix

measurement: observed position (same shape as H*x)

R: measurement noise (same shape as H)

motion: external motion added to state vector x

Q: motion noise (same shape as P)

F: next state function: x_prime = F*x

H: measurement function: position = H*x

Return: the updated and predicted new values for (x, P)

See also http://en.wikipedia.org/wiki/Kalman_filter

This version of kalman can be applied to many different situations by

appropriately defining F and H

'''

# UPDATE x, P based on measurement m

# distance between measured and current position-belief

y = np.matrix(measurement).T - H * x

S = H * P * H.T + R # residual convariance

K = P * H.T * S.I # Kalman gain

x = x + K*y

I = np.matrix(np.eye(F.shape[0])) # identity matrix

P = (I - K*H)*P

# PREDICT x, P based on motion

x = F*x + motion

P = F*P*F.T + Q

return x, P

def demo_kalman_xy():

x = np.matrix('0. 0. 0. 0.').T

P = np.matrix(np.eye(4))*1000 # initial uncertainty

N = 20

true_x = np.linspace(0.0, 10.0, N)

true_y = true_x**2

observed_x = true_x + 0.05*np.random.random(N)*true_x

observed_y = true_y + 0.05*np.random.random(N)*true_y

plt.plot(observed_x, observed_y, 'ro')

result = []

R = 0.01**2

for meas in zip(observed_x, observed_y):

x, P = kalman_xy(x, P, meas, R)

result.append((x[:2]).tolist())

kalman_x, kalman_y = zip(*result)

plt.plot(kalman_x, kalman_y, 'g-')

plt.show()

demo_kalman_xy()

红点显示嘈杂的位置测量，绿线显示卡尔曼预测位置。