Filter 卡尔曼滤波收敛

附件是一个简单的python Kalman过滤器示例，用于自由落体（g=-9.8m/s^2） 唉，我有个问题。状态向量x包含位置和速度，但z向量（测量）仅包含位置

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

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

附件是python代码：

---

在您的代码中，我看到两个问题

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

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

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

第二个问题是测量噪声。模拟R=100的噪声测量，但与滤波器R=4通信。过滤器对测量值的信任度高于其应有的值。这个问题与您的问题并不相关，但仍应予以纠正

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

以下是R=4的估算值：

对于R=100：

更新

速度估计是错误的，因为矩阵运算中有一些错误。请注意，矩阵乘法通过

np.dot（）

，而不是通过

*

以下是

v0=20

的正确结果：

非常感谢，安东

为方便起见，以下是更正后的代码：

投资回报率

---

非常感谢。我改变了你说的，但是当我绘制速度图时，它不会收敛到实际值。如果它们通过F矩阵关联，那怎么可能呢？仍然有一些使用

*

的计算。请查看带有

K*

的行。还有一些使用

*

进行的计算，对于您的情况来说是可以的，但是对于更广泛的情况，它将返回错误的结果。看看

K=np.dot（P，H.T）*S2

S

是一个常见情况下的矩阵，所以我也会使用

np.dot（）

。与

B*u

相同，它们在更复杂的示例中也是矩阵。因此，请确保您交换了所有此类操作。我通常使用Matlab，没有这样的问题。

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]")