卡尔曼滤波器收敛

Question

附件是自由落体物体的简单python卡尔曼滤波器示例(g=-9.8m/s^2) 唉，我有个问题。状态向量 x 包含位置和速度，但 z 向量（测量值）仅包含位置。

如果我设置了 错误的 初始位置值，即使有噪声测量，算法也会覆盖真实值（见下图）

但是，如果我发送了错误的初始速度值，即使运动模型定义正确，算法也不会收敛。

附件是python代码： kalman.py

Answer 1

在你的代码中我发现了两个问题。

您将 Q 矩阵设置为零。这意味着您过于信任您的模型，并且没有给过滤器任何机会通过测量来改进估计。你的过滤器变得僵硬。你可以把它想象成一个时间常数很大的低通滤波器。

在我的代码中，我将 Q 矩阵设置为

Q = np.array([[1,0],[0,0.1]])

第二个问题是您的测量噪音。您使用 R=100 模拟噪声测量，但与滤波器 R=4 通信。过滤器比应有的更信任测量。这个问题与你的问题没有太大关系，但仍然应该更正。

现在即使我将初始速度设置为20，位置估计也能正常工作。

这是 R = 4 的估计值：

对于 R = 100：

更新

速度估计不对，因为你的矩阵运算有一些错误。请注意，矩阵乘法通过 np.dot()，而不是通过 *.

这是 v0 = 20 的正确结果：

Answer 2

非常感谢，安东。

为了方便起见，下面附上更正后的代码：

投资回报率

import numpy as np
import matplotlib.pyplot as plt
%matplotlib notebook
from numpy.linalg import inv

N = 1000 # number of time steps
dt = 0.01 # Sampling time (s)
t = dt*np.arange(N)

F = np.array([[1, dt],[ 0, 1]])# system matrix - state
B = np.array([[-1/2*dt**2],[ -dt]])# system matrix - input
H = np.array([[1, 0]])#; % observation matrix
Q = np.array([[1,0],[0,1]]) 


u = 9.80665# % input = acceleration due to gravity (m/s^2)
I = np.array([[1,0],[0,1]])  #identity matrix

# Define the initial position and velocity
y0 = 100; # m
v0 = 0; # m/s
G2 = np.array([-1/2*dt**2, -dt])# system matrix - input

# Initialize the state vector (true state)
xt = np.zeros((2, N)) # True state vector

xt[:,0] = [y0,v0]

for k in range(1,N):
    xt[:,k] = np.dot(F,xt[:,k-1]) +G2*u

    #Generate the noisy measurement from the true state
R = 4 # % m^2/s^2
v = np.sqrt(R)*np.random.randn(N) #% measurement noise
z = np.dot(H,xt) + v; #% noisy measurement
R2=4


#% Initialize the covariance matrix
P = np.array([[10, 0], [0, 0.1]])# Covariance for initial state error
#% Loop through and perform the Kalman filter equations recursively

x_list =[]


x_kalman= np.array([[117],[290]])
x_list.append(x_kalman)
print(-B*u)
for k in range(1,N):
    x_kalman=np.dot(F,x_kalman) +B*u
    P = np.dot(np.dot(F,P),F.T) +Q
    S=(np.dot(np.dot(H,P),H.T) + R2)
    S2 = inv(S)
    K = np.dot(P,H.T)*S2
    x_kalman = x_kalman +K*((z[:,k]- np.dot(H,x_kalman)))
    P = np.dot((I - K*H),P)
    x_list.append(x_kalman)   
x_array = np.array(x_list)
print(x_array.shape)

plt.figure()
plt.plot(t,z[0,:], label="measurment", color='LIME', linewidth=1)
plt.plot(t,x_array[:,0,:],label="kalman",linewidth=5)
plt.plot(t,xt[0,:],linestyle='--', label = "Truth",linewidth=6)
plt.legend(fontsize=30)
plt.grid(True)
plt.xlabel("t[s]")
plt.title("Position Estimation", fontsize=20)
plt.ylabel("$X_t$ = h[m]")
plt.gca().set( ylim=(0, 110))
plt.gca().set(xlim=(0,6))

plt.figure()
#plt.plot(t,z, label="measurment", color='LIME')
plt.plot(t,x_array[:,1,:],label="kalman",linewidth=4)
plt.plot(t,xt[1,:],linestyle='--', label = "Truth",linewidth=2)
plt.legend()
plt.grid(True)
plt.xlabel("t[s]")
plt.title("Velocity Estimation")
plt.ylabel("$X_t$ = h[m]")

卡尔曼滤波器收敛

Kalman FIlter Convergence

filter

particles

kalman-filter