udacity 卡尔曼滤波(KF)-python版本

udacity 2维EKF Python根据网上的C++版本改写，方便大家参考。

用于处理激光数据。

一、引用的库

import random
from math import *
import matplotlib.pyplot as plt
import decimal

二、定义矩阵类

来自网络资料。

class matrix:
    def __init__(self, value):
        self.value = value
        self.dimx = len(value)
        self.dimy = len(value[0])
        if value == [[]]:
            self.dimx = 0

    def zero(self, dimx, dimy):
        # check if valid dimensions
        if dimx < 1 or dimy < 1:
            raise ValueError("Invalid size of matrix")
        else:
            self.dimx = dimx
            self.dimy = dimy
            self.value = [[0 for row in range(dimy)] for col in range(dimx)]

    def identity(self, dim):
        # check if valid dimension
        if dim < 1:
            raise ValueError("Invalid size of matrix")
        else:
            self.dimx = dim
            self.dimy = dim
            self.value = [[0 for row in range(dim)] for col in range(dim)]
            for i in range(dim):
                self.value[i][i] = 1

    def show(self):
        for i in range(self.dimx):
            print(self.value[i])
        print(' ')

    def __add__(self, other):
        # check if correct dimensions
        if self.dimx != other.dimx or self.dimy != other.dimy:
            raise ValueError("Matrices must be of equal dimensions to add")
        else:
            # add if correct dimensions
            res = matrix([[]])
            res.zero(self.dimx, self.dimy)
            for i in range(self.dimx):
                for j in range(self.dimy):
                    res.value[i][j] = self.value[i][j] + other.value[i][j]
            return res

    def __sub__(self, other):
        # check if correct dimensions
        if self.dimx != other.dimx or self.dimy != other.dimy:
            raise ValueError("Matrices must be of equal dimensions to subtract")
        else:
            # subtract if correct dimensions
            res = matrix([[]])
            res.zero(self.dimx, self.dimy)
            for i in range(self.dimx):
                for j in range(self.dimy):
                    res.value[i][j] = self.value[i][j] - other.value[i][j]
            return res

    def __mul__(self, other):
        # check if correct dimensions
        if self.dimy != other.dimx:
            raise ValueError("Matrices must be m*n and n*p to multiply")
        else:
            # multiply if correct dimensions
            res = matrix([[]])
            res.zero(self.dimx, other.dimy)
            for i in range(self.dimx):
                for j in range(other.dimy):
                    for k in range(self.dimy):
                        res.value[i][j] += self.value[i][k] * other.value[k][j]
            return res

    def transpose(self):
        # compute transpose
        res = matrix([[]])
        res.zero(self.dimy, self.dimx)
        for i in range(self.dimx):
            for j in range(self.dimy):
                res.value[j][i] = self.value[i][j]
        return res

        # Thanks to Ernesto P. Adorio for use of Cholesky and CholeskyInverse functions

    def Cholesky(self, ztol=1.0e-5):
        # Computes the upper triangular Cholesky factorization of
        # a positive definite matrix.
        res = matrix([[]])
        res.zero(self.dimx, self.dimx)

        for i in range(self.dimx):
            S = sum([(res.value[k][i]) ** 2 for k in range(i)])
            d = self.value[i][i] - S
            if abs(d) < ztol:
                res.value[i][i] = 0.0
            else:
                if d < 0.0:
                    raise ValueError("Matrix not positive-definite")
                res.value[i][i] = sqrt(d)
            for j in range(i + 1, self.dimx):
                S = sum([res.value[k][i] * res.value[k][j] for k in range(self.dimx)])
                if abs(S) < ztol:
                    S = 0.0
                res.value[i][j] = (self.value[i][j] - S) / res.value[i][i]
        return res

    def CholeskyInverse(self):
        # Computes inverse of matrix given its Cholesky upper Triangular
        # decomposition of matrix.
        res = matrix([[]])
        res.zero(self.dimx, self.dimx)

        # Backward step for inverse.
        for j in reversed(range(self.dimx)):
            tjj = self.value[j][j]
            S = sum([self.value[j][k] * res.value[j][k] for k in range(j + 1, self.dimx)])
            res.value[j][j] = 1.0 / tjj ** 2 - S / tjj
            for i in reversed(range(j)):
                res.value[j][i] = res.value[i][j] = -sum(
                    [self.value[i][k] * res.value[k][j] for k in range(i + 1, self.dimx)]) / self.value[i][i]
        return res

    def inverse(self):
        aux = self.Cholesky()
        res = aux.CholeskyInverse()
        return res

    def __repr__(self):
        return repr(self.value)

三、初始化变量

x = matrix([[0.], [0.], [0.], [0.]]) # initial state (location and velocity)
P = matrix([[1., 0, 0, 0.],[0, 1, 0, 0],[0, 0, 1000, 0], [0, 0, 0., 1000.]]) # initial uncertainty
u = matrix([[0.], [0.]]) # external motion
F = matrix([[1., 0., 1., 0.], [0., 1., 0., 1.],[0., 0., 1., 0.],[0., 0., 0., 1.]]) # next state function
H = matrix([[1., 0., 0, 0],[0, 1, 0, 0]]) # measurement function
R = matrix([[0.0225, 0],[0, 0.0225]]) # measurement uncertainty
Q = matrix([[0, 0, 0, 0],[0, 0, 0, 0],[0, 0, 0, 0],[0, 0, 0, 0]])
I = matrix([[1., 0., 0, 0], [0., 1., 0, 0],[0, 0, 1, 0],[0, 0, 0, 1]]) # identity matrix
noise_ax = 5
noise_ay = 5

四、2D KF

def kalman_filter_2D(measurement):

    global x
    global P
    # predict
    x = F * x
    P = F * P * F.transpose() + Q

    # measurement update
    Z = measurement
    y = Z - (H * x)
    S = H * P * H.transpose() + R
    K = P * H.transpose() * S.inverse()

    x = x + (K * y)
    P = (I - (K * H)) * P


    return x, P

五、数据处理

注：以下程序用于处理激光数据。

time = 0.
b_init = 0

new_x = []
new_y = []
pre_t = 0.
dt = 0.
dt2 = 0.
dt3 = 0.
dt4 = 0.

meas = matrix([[0.], [0.]]) # initial measurement
file1 = open('obj_pose-laser-radar-synthetic-input.txt', 'r')
file2 = open('obj_pose-laser-radar-synthetic-output.txt', 'w+')

data = file1.readlines()

for line in data:
    line = line.split('\t')
    sensor = line[0]
    if(sensor == 'L'):
        meas.value[0][0] = float(line[1])
        meas.value[1][0] = float(line[2])

        time = float(line[3])

        if(b_init == 0):
            b_init = 1
            pre_t = time
            x.value[0][0] = meas.value[0][0]
            x.value[1][0] = meas.value[1][0]
            file2.write(str(x.value[0][0]) + ' ' + str(x.value[1][0]) + ' ' + str(x.value[2][0]) + ' ' + str(x.value[3][0]) + '\n')
        else:
            dt = (time - pre_t) / 1000000.0
            pre_t = time

            dt2 = dt * dt
            dt3 = dt2 * dt
            dt4 = dt3 * dt

            F.value[0][2] = dt
            F.value[1][3] = dt


            Q.value[0][0] = dt4 * noise_ax / 4
            Q.value[0][2] = dt3 * noise_ax / 2
            Q.value[1][1] = dt4 * noise_ay / 4
            Q.value[1][3] = dt3 * noise_ay / 2
            Q.value[2][0] = dt3 * noise_ax / 2
            Q.value[2][2] = dt2 * noise_ax
            Q.value[3][1] = dt3 * noise_ay / 2
            Q.value[3][3] = dt2 * noise_ay


            kalman_filter_2D(meas)
            file2.write(str(x.value[0][0]) + ' ' + str(x.value[1][0]) + ' ' + str(x.value[2][0]) + ' ' + str(x.value[3][0]) + '\n')

file1.close()
file2.close()

六、图形显示

import decimal
import matplotlib.pyplot as plt


m_px = []  #measurement position
m_py = []

gt_px = [] #groudtruth position
gt_py = []

gt_vx = [] #groundtruth velocity
gt_vy = []

e_px = []  #estimation position
e_py = []

e_vx = []  #estimation velocity
e_vy = []

with open('obj_pose-laser-radar-synthetic-input.txt', 'r') as file:
    data = file.readlines()
for line in data:
    line = line.split('\t')
    sensor = line[0]
    if(sensor == 'L'):
        m_px.append(float(line[1]))
        m_py.append(float(line[2]))

        gt_px.append(float(line[4]))
        gt_py.append(float(line[5]))

        gt_vx.append(float(line[6]))
        gt_vy.append(float(line[7]))

with open('obj_pose-laser-radar-synthetic-output.txt', 'r') as file:
    data = file.readlines()
for line in data:
    line = line.split(' ')
    e_px.append(float(line[0]))
    e_py.append(float(line[1]))
    e_vx.append(float(line[2]))
    e_vy.append(float(line[3]))

'''
plt.scatter(m_px, m_py, c='red',linewidth = 1, s = 5)
plt.scatter(e_px,e_py,c='green',linewidth = 1, s = 5)
plt.scatter(gt_px,gt_py,c='blue',linewidth = 1, s = 5)
'''

plt.scatter(e_vx,e_vy,c='blue')
plt.scatter(gt_vx,gt_vy,c='yellow',linewidth = 1, s = 5)

plt.show()

print("end")

七、对比

Python版本代码运行结果与网上的C++版本运行结果一致。

八、关于速度

实际上，状态变量由轨迹和速度组成，上述代码能够得到速度预测值。绿色点表示目标移动轨迹，蓝色值表示目标移动速度。

下图更明显，黄色为目标速度真值，蓝色为目标速度预测值。

九、关于dt

本次数据中，发现激光雷达测量的时间间隔dt不变，为0.1；

十、关于Q

如果不考虑Q，预测轨迹如下图示，红色为观测轨迹，绿色为预测轨迹，差距明显。需要考虑Q。

十一、关于noise_ax,noise_ay

如何取值？

noise_ax = 3
noise_ay = 3
与
noise_ax = 5
noise_ay = 5
结果相差不大，从预测轨迹上看不出差距。

即使取1000，似乎偏差也不大。

十二、关于R

R的取值对轨迹预测影响挺大。

十三、目标位置&速度的真值、测量值、预测值对比

只采用激光传感器对目标位置以及速度进行预测，如下图：

（1）x方向预测对比

（2）y方向预测对比

（3）目标位置轨迹以及速度分布对比

（4）结论：

采用激光雷达对于速度的预测似乎也可以。

十四、计算RMSE

代码如下：

def calculate_rmse(est, truth):
    rmse = matrix([[0., 0., 0., 0.]])
    residual = matrix([[0., 0., 0., 0.]])

    if est.dimx != truth.dimx:
        print("error")
        return rmse

    for i in range(est.dimx):
        for j in range(est.dimy):
            residual.value[0][j] = est.value[i][j] - truth.value[i][j]
            residual.value[0][j] = residual.value[0][j] * residual.value[0][j]
            rmse.value[0][j] = rmse.value[0][j] + residual.value[0][j]
            #rmse2.value[i][j] = math.sqrt(residual.value[0][j])

    rmse = matrix([[math.sqrt(rmse.value[0][0] / est.dimx), math.sqrt(rmse.value[0][1] / est.dimx),
                    math.sqrt(rmse.value[0][2] / est.dimx), math.sqrt(rmse.value[0][3] / est.dimx)]])

    return rmse

十五、Lidar测量值、预测值与真值对比

(1)测量值与真值的RMSE

备注：这里测量值只包含位置的测量值，没有速度的测量值；导致速度的RMSE较大。

(2)预测值与真值的RMSE

备注：预测值能够得到速度的预测值

(3)小结

采用卡尔曼滤波后降低了RMSE

十六、各种RMSE对比

(1)RMSE:X方向轨迹预测值与真值比较

(2)RMSE:Y方向轨迹预测值与真值比较

(3)RMSE:X方向速度预测值与真值比较

(4)RMSE:Y方向速度预测值与真值比较

(5)RMSE:所有RMSE比较