使用EKF融合IMU数据和编码器数据 - 知乎
自动泊车APA需要短程高精度的定位，受限于平台算力，找到一种基于EKF的IMU和轮速计融合方法。
待估计量：横摆角、加速度、速度和位置
观测值：速度、相邻两帧的角度差、距离
采用如下的公式计算状态转移方程：
观测模型：
求解雅可比矩阵F和H
其中，EKF计算如下：
代码如下：
def ekf(self,z_k_observation_vector, state_estimate_k_minus_1, control_vector_k,
control_vector_k_minus_1, P_k_minus_1, dt,s):
q_k_minus_1=state_estimate_k_minus_1[0]
v_x_k_minus_1=state_estimate_k_minus_1[1]
v_y_k_minus_1=state_estimate_k_minus_1[2]
p_x_k_minus_1=state_estimate_k_minus_1[3]
p_y_k_minus_1=state_estimate_k_minus_1[4]
a_x_k=control_vector_k[0]
a_y_k=control_vector_k[1]
w_k=control_vector_k[2]
a_x_k_minus_1=control_vector_k_minus_1[0]
a_y_k_minus_1=control_vector_k_minus_1[1]
w_k_minus_1=control_vector_k_minus_1[2]
R_k_minus_1=np.array([[math.cos(q_k_minus_1),-math.sin(q_k_minus_1)],
[math.sin(q_k_minus_1),math.cos(q_k_minus_1)]])
w_m=1/2*(w_k+w_k_minus_1)
q_k=q_k_minus_1+w_m*dt+self.process_noise_v_k_minus_1[0]
R_k=np.array([[math.cos(q_k),-math.sin(q_k)],
[math.sin(q_k),math.cos(q_k)]])
a_m=1.0/2*(R_k_minus_1.dot(np.array([a_x_k_minus_1,a_y_k_minus_1]).T)+R_k.dot(np.array([a_x_k,a_y_k]).T))
v_k_minus_1=np.array([v_x_k_minus_1,v_y_k_minus_1])
p_k_minus_1=np.array([p_x_k_minus_1,p_y_k_minus_1])
v_k=v_k_minus_1+a_m.T*dt
v_x_k=v_k[0]+self.process_noise_v_k_minus_1[1]
v_y_k=v_k[1]+self.process_noise_v_k_minus_1[2]
p_k=p_k_minus_1+v_k_minus_1*dt+1.0/2*a_m.T*dt*dt
p_x_k=p_k[0]+self.process_noise_v_k_minus_1[3]
p_y_k=p_k[1]+self.process_noise_v_k_minus_1[4]
state_estimate_k=np.array([q_k,v_x_k,v_y_k,p_x_k,p_y_k])
F_k=np.array([[1,0,0,0,0],
[1/2*(-math.sin(q_k_minus_1)*a_x_k_minus_1-math.cos(q_k_minus_1)*a_y_k_minus_1)*dt,1,0,0,0],
[1.0/2*(math.cos(q_k_minus_1)*a_x_k_minus_1-math.sin(q_k_minus_1)*a_y_k_minus_1)*dt,0,1,0,0],
[1.0/4*(-math.sin(q_k_minus_1)*a_x_k_minus_1-math.cos(q_k_minus_1)*a_y_k_minus_1)*dt*dt,dt,0,1,0],
[1.0/4*(math.cos(q_k_minus_1)*a_x_k_minus_1-math.sin(q_k_minus_1)*a_y_k_minus_1)*dt*dt,0,dt,0,1]])
# F_k=np.array([[1,0,0,0,0],
# [1/2*(-math.sin(q_k_minus_1)*a_x_k_minus_1-math.cos(q_k_minus_1)*a_y_k_minus_1)*dt,1,0,0,0],
# [1.0/2*(math.cos(q_k_minus_1)*a_x_k_minus_1-math.sin(q_k_minus_1)*a_y_k_minus_1)*dt,0,1,0,0],
# [0,dt,0,1,0],
# [0,0,dt,0,1]])
theta_k=w_k*dt
H_k=np.array([[(((math.sin(q_k))-(math.cos(q_k)))*(p_x_k-p_x_k_minus_1+p_y_k-p_y_k_minus_1))/((math.cos(q_k)+math.sin(q_k))**2),0,0,1/(math.cos(q_k)+math.sin(q_k)),1/(math.cos(q_k)+math.sin(q_k))],
[1.0,0,0,0,0],
[0,v_x_k*math.pow((v_x_k**2+v_y_k**2),-1.0/2),v_y_k*math.pow((v_x_k**2+v_y_k**2),-1.0/2),0,0]])
# H_k=np.array([[(((math.sin(q_k)))*(p_x_k-p_x_k_minus_1))/((math.cos(q_k))**2),0,0,1/(math.cos(q_k)),0],
# [1.0,0,0,0,0],
# [0,v_x_k*math.pow((v_x_k**2+v_y_k**2),-1.0/2),v_y_k*math.pow((v_x_k**2+v_y_k**2),-1.0/2),0,0]])
P_k=(F_k .dot(P_k_minus_1)).dot(F_k.T) +self.Q_k
# if s==0:
# v=math.pow((v_x_k**2+v_y_k**2),1/2 )
# else:
# v=-math.pow((v_x_k**2+v_y_k**2),1/2 )
measurement_residual_y_k=np.array([z_k_observation_vector[0]-(p_x_k-p_x_k_minus_1+p_y_k-p_y_k_minus_1)/(math.cos(q_k_minus_1)+math.sin(q_k_minus_1))+self.sensor_noise_w_k[0],z_k_observation_vector[1]-(q_k-q_k_minus_1)+self.sensor_noise_w_k[1],z_k_observation_vector[2]-math.pow((v_x_k**2+v_y_k**2),1/2 )+self.sensor_noise_w_k[2]])
# measurement_residual_y_k=np.array([z_k_observation_vector[0]-(p_x_k-p_x_k_minus_1)/(math.cos(q_k))+self.sensor_noise_w_k[0],z_k_observation_vector[1]-(q_k-q_k_minus_1)+self.sensor_noise_w_k[1],z_k_observation_vector[2]-math.pow((v_x_k**2+v_y_k**2),1/2 )+self.sensor_noise_w_k[2]])
S_k = H_k @ P_k @ H_k.T + self.R_k
K_k = P_k @ H_k.T @ np.linalg.pinv(S_k)
state_estimate_k = state_estimate_k + (K_k @ measurement_residual_y_k)
P_k = P_k - (K_k @ H_k @ P_k)
return state_estimate_k, P_k
process_noise_v_k_minus_1=np.array([0,0,0,0,0])
sensor_noise_w_k=np.array([0,0,0])
# # q、vx、vy、px、py
# Q_k=np.array([[10000,0,0,0,0],
# [0,1000,0,0,0],
# [0,0,1000,0,0],
# [0,0,0,0.00001,0],
# [0,0,0,0,0.00001]])
# # s,theta,v
# R_k=np.array([[0.01,0,0],
# [0,0.01,0],
# [0,0,0.01]
# ])
# q、vx、vy、px、py
Q_k=np.array([[1,0,0,0,0],
[0,1,0,0,0],
[0,0,1,0,0],
[0,0,0,1,0],
[0,0,0,0,1]])
# s,theta,v
R_k=np.array([[1000,0,0],
[0,1000,0],
[0,0,1000]
])
def method_EKF(self):
time=self.get_time()
vline=self.vline_wheelspeed()
yaw_rate=self.get_yaw_rate()
lat=self.get_acc_lat()
lgt=self.get_acc_lgt()
state=self.get_moveing_state()
n=len(vline)
x=[]
y=[]
yaw=[]
vline_all=[]
# 初始??
# 后面会重新计算F和H矩阵
F_k=np.array([[1.0,0,0,0,0],
[0,1.0,0,0,0],
[0,0,1.0,0,0],
[0,0,0,1.0,0],
[0,0,0,0,1.0]])
H_k=np.array([[1.0,0,0,0,0],
[0,1.0,0,0,0],
[0,0,1.0,0,0],
[0,0,0,1.0,0],
[0,0,0,0,1.0]])
state_estimate_k_minus_1 = np.array([0.0,0.0,0.0,0.0,0.0])
P_k_minus_1=np.array([[1.0,0,0,0,0],
[0,1.0,0,0,0],
[0,0,1.0,0,0],
[0,0,0,1.0,0],
[0,0,0,0,1.0]])
for i in range(1,n):
s=state[i]
dt=time[i]-time[i-1]
if state[i]==0:
v=vline[i];
elif state[i]==1:
v=-vline[i]
else:
v=0.0
# 观测值s,theta,v
obs_vector_z_k=np.array([v*dt,yaw_rate[i]*dt,math.fabs(v)])
# 控制量ax,ay,w
control_vector_k_minus_1=np.array([lgt[i-1],lat[i-1],yaw_rate[i-1]])
control_vector_k=np.array([lgt[i],lat[i],yaw_rate[i]])
# 计算优化后的状态量和协方差矩阵
optimal_state_estimate_k,covariance_estimate_k = self.ekf(obs_vector_z_k,state_estimate_k_minus_1, control_vector_k,
control_vector_k_minus_1, P_k_minus_1, dt,s)
# 更新k-1时刻的状态量和协方差矩阵
state_estimate_k_minus_1 = optimal_state_estimate_k
P_k_minus_1 = covariance_estimate_k
x.append(optimal_state_estimate_k[3])
y.append(optimal_state_estimate_k[4])
yaw.append(optimal_state_estimate_k[0])
vline_all.append(math.pow(optimal_state_estimate_k[1]**2+optimal_state_estimate_k[2]**2,1.0/2))
return x,y,yaw,vline_all
实验结果一般，Q和R矩阵较难设置，同时轮速和加速度计本身并不准确，在进一步学习了EKF融合后有以下优化思路：
1、不再使用轮速，改用轮速脉冲和标定准确的轮胎直径
2、将加速度的零偏作为待估计量，不必时刻update，等脉冲发生变化后再进行update
因为后续项目中采用了基于标定的方法，该方案暂时搁置，待有空后再进行新的尝试。
文章知识点与官方知识档案匹配，可进一步学习相关知识算法技能树首页概览44163 人正在系统学习中