udacity 扩展卡尔曼滤波（EKF）-python版本

capitel2

已于 2024-06-15 20:29:06 修改

阅读量239

点赞数 3

文章标签：算法

于 2024-04-26 14:46:46 首次发布

本文链接：https://blog.csdn.net/capitel2/article/details/138217983

版权

本程序用于处理毫米波雷达数据。参考udacity资料以及网络资料。供大家参考。

一、关键公式推导

二、正态分布数据生成

import numpy as np
import matplotlib.pyplot as plt
import math

# 设置随机种子以获得可复现的结果
np.random.seed(0)

# 设置正态分布的参数
mean, std = 0, 3

# 生成正态分布的随机数
data = np.random.normal(mean, std, size = 10000)

plt.xlim(-10, 10)  # x轴的范围设置为-3到3
plt.ylim(0, 0.2)  # y轴的范围设置为0到30

# 计算直方图
plt.hist(data, bins=300, density=True, alpha=0.5, color='red', edgecolor='black')

# 设置x和y轴的标签
plt.xlabel('Value')
plt.ylabel('Probability')

# 设置图表标题
plt.title('Histogram of Normal Distribution')

# 显示图表
plt.show()

三、正态分布数据的非线性变换

data = np.random.normal(mean, std, size = 10000)

new_data = []
for i in data:
    new_data.append(math.atan(i)) #non linear transform

非线性变换后的数据分布图（非正态分布）

四、正态分布数据的线性变换

# 生成正态分布的随机数
data = np.random.normal(mean, std, size = 10000)

new_data = []
new_data2 = []
for i in data:
    new_data2.append(1.2*i)   #linear transform

线性变换后的数据分布图（近似正态分布）

五、初始化变量

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],[0, 0, 1, 0]]) # measurement function
R = matrix([[0.09, 0,  0],[0, 0.0009, 0],[0, 0, 0.09]]) # 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 = 9
noise_ay = 9

六、雅可比变换

def calculate_jacobian(x):
    Hj = matrix([[0., 0., 0., 0.], [0., 0., 0., 0.],[0., 0., 0., 0.]])

    px = x.value[0][0]
    py = x.value[1][0]
    vx = x.value[2][0]
    vy = x.value[3][0]

    c1 = px * px + py * py
    c2 = sqrt(c1)
    c3 = c1 * c2

    if(fabs(c1) < 0.0001):
        print("error")
        return Hj

    Hj = matrix([[px/c2, py/c2, 0, 0],[-py/c1, px/c1, 0, 0],[ py * (vx * py - vy * px) / c3, px * (px * vy - py * vx) / c3, px / c2, py / c2]])
    return Hj

七、坐标变换

def RadarCartesianToPolar(x):
    px = x.value[0][0]
    py = x.value[1][0]
    vx = x.value[2][0]
    vy = x.value[3][0]

    rho = sqrt(px * px + py * py)
    phi = atan2(py, px)

    if (rho < 0.000001):
        rho = 0.000001

    rho_dot = (px * vx + py * vy) / rho

    z_pred = matrix([[rho], [phi], [rho_dot]])
    return z_pred

八、2D EKF

def extend_kalman_filter_2D(measurement):

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

    # measurement update   
    Z_pred = RadarCartesianToPolar(x)
    Z = measurement
    y = Z - Z_pred

    while (y.value[1][0] > 3.14159265):
        y.value[1][0] = y.value[1][0] - 3.14159265 * 2

    while (y.value[1][0] < -3.14159265):
        y.value[1][0] = y.value[1][0] + 3.14159265 * 2

    H = calculate_jacobian(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

pre_t = 0.
dt = 0.
dt2 = 0.
dt3 = 0.
dt4 = 0.

meas = matrix([[0.], [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 == 'R'):
        meas.value[0][0] = float(line[1]) #rho
        meas.value[1][0] = float(line[2]) #phi
        meas.value[2][0] = float(line[3]) #rho-dot

        time = float(line[4])

        if(b_init == 0):
            b_init = 1
            pre_t = time
            x.value[0][0] = meas.value[0][0] * cos(meas.value[1][0]) #px = rho * cos(phi)
            x.value[1][0] = meas.value[0][0] * sin(meas.value[1][0]) #py = roh * sin(phi)
            x.value[2][0] = 0.
            x.value[3][0] = 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


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

十、结果对比

本程序参考的C++代码：

GitHub - JunshengFu/tracking-with-Extended-Kalman-Filter: Object (e.g Pedestrian, vehicles) tracking by Extended Kalman Filter (EKF), with fused data from both lidar and radar sensors.

本程序得到的输出结果与参考的C++代码一致。

十一、关于速度的初始化

网上有不同的C++版本对速度进行初始化

（1）版本1

float ro     = measurement_pack.raw_measurements_(0);
float phi    = measurement_pack.raw_measurements_(1);
float ro_dot = measurement_pack.raw_measurements_(2);
ekf_.x_(0) = ro     * cos(phi);
ekf_.x_(1) = ro     * sin(phi);
ekf_.x_(2) = ro_dot * cos(phi);
ekf_.x_(3) = ro_dot * sin(phi);

（2）版本2(参考的C++代码)

// phi is not the direction of the speed, it is better to set vx and vy to 0
ekf_.x_ << rho * cos(phi), rho * sin(phi), 0, 0; // x, y, vx, vy

个人认为版本2是对的。

十二、关于角度值的标准化处理

Normalizing Angles
In C++, atan2() returns values between -pi and pi. When calculating phi in y = z - h(x) for radar measurements,the resulting angle phi in the y vector should be adjusted so that it is between -pi and pi. The Kalman filter is expecting small angle values between the range -pi and pi.

有些版本的C++代码没有做角度的标准化处理，得到的轨迹预测值是不正确的。

不同的版本处理也不一样。

（1）版本1

// Assume this is radar measurement，/ y(1) is an angle (phi), it shoulde be normalized
if (y.size() == 3) y(1) = atan2(sin(y(1)), cos(y(1)));

（2）版本2

if(y(1) > PI){
   y(1) = y(1) - 2*PI;
   }
else if(y(1) < -PI){
   y(1) = y(1) + 2*PI;
   }

（3）版本3（参考的C++版本）

// normalize the angle between -pi to pi
while(y(1) > M_PI){
y(1) -= PI2;
}

while(y(1) < -M_PI){
y(1) += PI2;
}

个人认为版本3是对的。

十三、Radar测量值、预测值与真值对比

(1)测量值与真值的RMSE