Kalman实际应用总结

Kalman理论介绍
- 一. 简单理论介绍理论
- 二. 升华理论介绍
Kalman基本应用
本文总结

理论部分不详细说明，网上大部分都给出很好的解释

网上大部分都是理论和简单的例子，很少看到实战的信息

本博文是笔者实际使用的总结，如有错误，请不吝指教

Kalman理论介绍

一. 简单理论介绍理论

二. 升华理论介绍

Kalman基本应用

一. Kalman跟踪/滤波

对单个数据滤波，无法建立运动学模型

通过建立和自身相关的状态方程即可

是一种平滑操作（上一时刻和当前时刻的关系）

举例：

对一个平面运动的质点进行跟踪（$X、Y$）？

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

求解：

fig = plt.figure()

axis = fig.add_subplot(1,1,1)

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 $推导可知

举例一：

问题和例子文档

一个运动小车的位置和速度的测量等信息可以被测量(一个传感器)，也可以通过牛顿运动学方程进行解算，这两个到底谁占的比例高？使用Kalman的协方差矩阵进行比例的计算。。。。具体看文档

举例二：

stackoverflow上的一个例子

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

这部分比较简单，网上的例子大部分都是基于此的。。。

三. Kalman多传感器融合A

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

举例：

以汽车跟踪为例，目标是知道汽车时刻的状态$x=(p_x,p_y,v_x,v_y)$$x=(p_x,p_y,v_x,v_y)$

已知的传感器有$、lidar、radar$。

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

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

这是优达学城的一个例子，具体我也没视频网址。

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

注意：

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

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

四. Kalman多传感器融合B

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

举例：

一个小车做不均则运动（速度、加速度、角速度等都是可变的），现在有两个传感器：仪器A和仪器B，他们都能测量 $\omega$ 和 $v$ ,那么如何进行融合两个传感器呢？

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

这里其实和Kalman的滤波比较类似，就是把两个传感器当做一个传感器的不同时间序列 $T_1,T_2$ 时刻测量的数据，然后滤波操作。

五. Kalman多传感器融合C

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

条件和Kalman多传感器融合B相同，单处理方式不同

由于部分传感器精度不同，进行特定的取舍很有必要（亲身经历）

假设求取小车的 $\omega$ 和 $v$

传感器A对$\omega$ 测量较为准确

传感器B对 $v$ 测量较为准确

解决：

其实我们如果直接按照Kalman多传感器融合B进行操作的话，误差基本不会缩小，可能还会增加

这个时候笔者的解决方案是把传感器A和B当做一个整体传感器C，传感器C测量的 $\omega$ 是A的，测量的 $v$ 是B的

那么我们就把这个合起来的传感器C进行滤波就行了

实测可用。。。

六. Kalman多传感器融合D

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

看到网上很多人问这个问题，这里笔者没有亲自实现，只是做了猜想，不正确还望读者指正

解决：

由于卡尔曼只能一次融合两个信息（预测和观测），所以只能进行如下想法

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

七. Extend Kalman

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

注意：

笔者认为这种情况比较少见，因为 $t$ 趋向于 $\epsilon$ ，所以可以认为在无穷小的区间都近似于很恒定的

实在没办法的时候就使用EKF，原理都很简单，计算代价大许多

后续可以使用UKF进行操作，这部分笔者还未尝试

本文总结

最后来个简单的总结，什么是卡尔曼$K$？

两个相同信息：A 和 B

都满足$y=kx+b$

那么如何得到 $y$ ?

正常来说：$y=(A+B)/2*x+b$

但是好像不是非常好，这个$(A+B)/2$总是不变的，假如他们某个时刻占比改变了呢？

这个时候$Kalman$的作用的体现了，他计算A和B的关系（看公式吧）

得出一个系数 $K$ 这个$K$ 和A、B相关

此时：$y=K*x+b$

输入的A、B不同，那么$K$也不同

完毕！！！