## Kalman基本应用

### 一. Kalman跟踪/滤波

• 速度，加速度，角速度$v、\alpha，\omega$都是未知状态

fig = plt.figure()

func_data = lambda x : x + x^2
z = np.mat(func_data(np.arange(1,100)))
x_mat = np.mat([[0,],[0.]])#状态矩阵[x,delta_x]
p_mat = np.mat([[1, 0], [0, 1]])#状态协方差矩阵
f_mat = np.mat([[1, 1],[0.,1.]])#状态转移矩阵
q_mat = np.mat([[0.0001, 0], [0, 0.0001]])
h_mat = np.mat([1.,0])# 观测矩阵[x]
r_mat = np.mat([1])#观测协方差矩阵
result = []
for i in range(z.shape[1]):
x_predict = f_mat * x_mat
p_predict = f_mat * p_mat * f_mat.T + q_mat

kalman = p_predict * h_mat.T / (h_mat * p_predict * h_mat.T + r_mat)

x_mat = x_predict + kalman *(z[0, i] - h_mat * x_predict)
p_mat = (np.eye(2) - kalman * h_mat) * p_predict
result.append(x_predict[0,0])

axis.plot(result,label='predict')
axis.plot(z.tolist()[0],label='groundtruth')
axis.legend()


### 二. Kalman预测/融合（单传感器）

• [x] 运动学模型
• [x] 单一传感器
• [x] 速度$、，v、\alpha，\omega$推导可知

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)

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()


### 三. Kalman多传感器融合A

• [x] 运动学模型
• [x] 多个传感器
• [x] 传感器时间序列不同

$lidar$：笛卡尔坐标系。可检测到位置，没有速度信息。其测量值$z=(px,py)z=(px,py)$

$radar$：极坐标系。可检测到距离，角度，速度信息，但是精度较低。其测量值$z=(ρ,ϕ,ρ˙)z=(ρ,ϕ,ρ˙)$

$matlab$代码地址在这里$python$代码在这里

​ 这里相当于建立了两个模型，一个线性模型，一个非线性模型，在不同的时刻使用不同的传感器进行更新

​ 其实就是单个传感器合并到一起了。。。。

### 四. Kalman多传感器融合B

• [ ] 无运动学模型
• [x] 多传感器
• [x] 传感器时序相同

• 具体的代码这里不方便给出，有需要可以一起讨论

• [ ] 无运动学模型
• [x] 多传感器
• [x] 传感器时序相同

### 六. Kalman多传感器融合D

• [x] 运动学模型
• [x] 多传感器
• [x] 传感器时序相同

1. 进行两次融合，一次是预测和传感器A，一次结果和传感器B（这部分就是多传感器B
2. 进行一次融合，预测和新的传感器C（Kalman多传感器融合C

### 七. Extend Kalman

• 运动学模型不是线性的
• 使用雅克比代替状态矩阵观测矩阵

## 本文总结

posted on 2019-08-17 21:58  影醉阏轩窗  阅读(171)  评论(0编辑  收藏

### 导航

/* 线条鼠标集合 */ /* 鼠标点击求赞文字特效 */ //带头像评论