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

本程序用于处理毫米波雷达数据。参考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 = matri