滤波笔记三：无迹卡尔曼滤波（UKF）
参考资料：
无迹卡尔曼滤波（UKF）超详细解释_咸鱼.m的博客-CSDN博客_ukf
https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python
无迹卡尔曼滤波(UKF)_别用我ID的博客-CSDN博客_ukf滤波算法
无迹卡尔曼滤波器详解_一抹烟霞的博客-CSDN博客_无迹卡尔曼滤波
3月16日 CV,CA,CTRV等运动模型，EKF，UKF在运动模型下的分析与实践_Hali_Botebie的博客-CSDN博客_cv运动模型
滤波笔记二：无迹卡尔曼滤波 CTRV&&CTRA模型_泠山的博客-CSDN博客_ctrv模型
Udacity-CarND UKF 学习笔记 |
Udacity的原视频：Introduction (learn-udacity.top)
补充参考：六.卡尔曼滤波器开发实践之六: 无损卡尔曼滤波器(UKF)进阶-白话讲解篇_okgwf的博客-CSDN博客_ukf
目录
引言
1.无损变换（无迹变换）Unscented Transformation
1.1 无迹变换原理以及Sigma点的选取
1.2 无迹卡尔曼滤波器的迭代过程（UKF in CTRV）
1.3 UKF的Python仿真代码
1.4 EKF和UKF的实验结果图对比
1.5 UKF参数修改后的实验结果
引言
非线性可以表现在两个方面：过程模型和测量模型。
用一个雷达测量对于目标的倾斜长度，取平方根来计算x，y的坐标。（其实我后面应该会直接读取卫星GPS的XY坐标数据）
雷达通过发射一束无线电波并通过扫描回波工作。任何物体都会在这束路径上返回一些信息给雷达。通过对于返回雷达所需时间的计算可以计算出倾斜距离（也就是直线距离）。
过程模型：跟踪一个飞行在空中的球，这个球受到重力与空气阻力的作用，因而具有高度非线性。
from kf_book.book_plots import set_figsize, figsize
import matplotlib.pyplot as plt
from kf_book.nonlinear_plots import plot_nonlinear_func
from numpy.random import normal
import numpy as np
plt.rcParams['font.sans-serif']=['SimHei'] #用来正常显示中文标签
plt.rcParams['axes.unicode_minus']=False #用来正常显示负号# ----------------------------过程模型------------------------------# 生成一组均值loc=0.0 标准差scale=1 数据点数为500000的满足正态分布的数据
data = normal(loc=0.0, scale=1, size=500000)# 定义一个非线性函数f(x)
def f(x):return (np.cos(4*(x/2 + 0.7))) - 1.3*x# 画出f(x) 输入input是data生成的符合高斯分布的数据点 输出output是输入经过f后得到的
plot_nonlinear_func(data,f)输入生成500000个采样点，通过非线性变换建立结果的分布直方图。
从输出的直方图我们可以计算出均值与标准差，这就是更新值，这些值是近似高斯分布的。
这意味着我们采样的过程就构成了我们问题的解。
再看一下输入输出的散点图：
data数据本身是符合高斯分布的，是散布在平均零点周围的白噪声。相反，f（data）是具有定义的结构。有两个带，中间有大量点。在带的外侧有分散的点，但在负侧有更多的点。
这个取样过程是我们问题的解决方案。假设对于每个更新，我们生成500000个点，通过函数传递它们，然后计算结果的均值和方差。这被称为蒙特卡罗方法，它被一些卡尔曼滤波器设计所使用，如集合滤波器和粒子滤波器。采样不需要专门知识，也不需要封闭形式的解决方案。无论函数是多么非线性或表现不佳，只要我们采样足够的sigma点，我们将建立精确的输出分布。
“足够的点数”才是关键。上面的图表是用500000个sigma点创建的，输出仍然不平滑。更糟糕的是，这只适用于一维。所需的点数随维数的幂而增加。如果一维只需要500个点，二维需要500个平方点，或250000个点，三维需要500个立方点，或125000000个点，以此类推。因此，尽管这种方法可行，但计算成本非常高。集成滤波器和粒子滤波器使用巧妙的技术来显著降低这种维数，但计算负担仍然很大。无迹卡尔曼滤波器使用Sigma点，但通过使用确定性方法选择点，大大减少了计算量。
1.无损变换（无迹变换）Unscented Transformation
引入：
先看一种2D的协方差椭圆的问题。
假设一些任意的非线性函数，我们会选取协方差椭圆中一些随机的点，通过非线性函数变化，并画出他们新的位置。左侧显示的是两个状态变量以一倍σ（方差）分布的椭圆。箭头表示的是几个随机选取的采样点通过非线性函数生成的新的分布。右侧的椭圆就是这些点的集合的均值与方差的估计。如果我们要去采样，比如说选取一百万个点，这些点的形状可能与椭圆相差很远。让我们通过非线性函数跑一些点进行观察。
考虑一个非线性系统：
10000个随机点符合的分布是：均值是0，协方差矩阵为
结果图：
该图显示了该函数具有很强的非线性，以及如果我们以EKF扩展卡尔曼滤波器的方式，在（0,0）点对其进行线性化将产生很大误差。虽然选取50000个点进行均值计算非常精确，但是计算50000个点会使我们的滤波器更新非常的慢。
那么我们能使用的最少的采样点是什么？制定的问题对Sigma于点的选择有哪些约束？
考虑到高斯分布的对称性，每次采样点的最小数目是三个点，一个点是均值，另外两个点分布在均值的两侧，如下图所示：
为了使采样点有效，我们希望采样点的权重之和等于1。因为如果总和大于或者小于1，则采样不会产生正确的输出。所以，我们必须控制sigma点以及其相应的权重，以便计算输入高斯的均值和方差。
1.1 无迹变换原理以及Sigma点的选取
无迹变换是UKF的核心，指的是在估计点附近确定采样，用这些采样点表示的高斯密度近似状态向量的概率密度函数。UKF的基本思想是卡尔曼滤波与无损变换，它能有效地克服EKF估计精度低、稳定性差的问题，因为不用忽略高阶项，所以对于非线性分布统计量的计算精度高。
UKF的要点：
UKF是对非线性函数的概率分布(均值μ和方差σ^2)进行近似(sigma points)，用一系列确定的样本来逼近状态的后验概率分布;
无迹变换的实现方法：在原来的状态分布中，按照某一规则选取一些采样点，使得这些采样点的均值和协方差等于原状态向量的分布的均值和协方差；将这些采样点代入非线性函数汇总，得到相应的非线性函数值的点集，通过这些点集求取非线性变换后的均值和协方差。这样得到的非线性变换后的均值和协方差的精度最少具有2阶精度。对于高斯分布，可以达到3阶精度。
Sigma采样点的选择是基于先验均值 和 先验协方差矩阵
的平方根的相关列实现的。
下面以对称分布采样、采用加性噪声的无迹变换为例，简要介绍无迹变换的基本原理。设一个非线性系统 y=f(x)，状态向量X是n维随机变量，且已知其均值为，协方差矩阵是
。则可以通过如下的无迹变换得到2n+1个Sigma点，以及其对应的权值w来计算y=f(x)的统计特征。
（1）选取sigma点，n代表状态向量的维数，i代表第几个sigma点。
第一个sigma点：
剩余的sigma点：
其中，λ是比例因子，其表达式为：。
根据公式，λ越大，sigma点就越远离状态向量的均值。λ越小，sigma点就越靠近状态向量的均值。公式中，
状态向量的均值周围的sigma点的分布状态由常数
决定，调节
可以调节高阶项的影响，通常设置为一个比较小的正数。K的取值没有具体的设定限制，但是至少要保证
是半正定矩阵，通常设定K=0.
（2）确定sigma点的权重
sigma点的权重应该满足以下性质：如果x是高斯分布,β = 2 是最佳的.
sigma点的权值分成两种，一种是求期望的时候的权值wm，另外一种是求方差的时候的权值wc。
而且只有第一个sigma点的这两种权值不一样，对于后面的2n的sigma点，这两种权值是一样的。
无迹变换是对非线性环节的处理。假如有这样一个非线性环节y=f(x)，我们在x附近取了很多Sigma点[x1,x2,...,xn],所以就有[y1,y2,...,yn]=[f(x1),f(x2),...,f(xn)]，然后y=w1*y1+w2*y2+...+wn*yn。
1.2 无迹卡尔曼滤波器的迭代过程（UKF in CTRV）
与线性kalman滤波一样，UKF采用过程模型对下一时刻的状态均值与协方差进行预测。
说明：有的地方的i是从0~2n ，有的地方的i是从1~2n+1，以下不做区分。
另外根据系统的噪声的存在方式，将其分为加性噪声和隐含噪声，对于两种噪声，UKF滤波的处理方式分为两种，分别是简化的UKF和扩维的UKF，这里仍然假设噪声是高斯分布的，以简化的UKF为例：
考虑如下非线性系统：
# ----------------------------------------------------------- 预测过程 ------------------------------------------------------
STEP1:生成当前时刻（k-1）具体的sigma点与其对应的权重.
假定状态的个数是n,则会产生 2n+1 个sigma点，其中第一个就是当前状态的均值μ，sigma点集的计算公式如上。
注意在CTRV模型之中，状态数量n除了要包含5个状态以外，还要包含处理噪声μa、μωˊ 因为这些处理噪声对模型也有非线性的影响。在增加了处理噪声的影响以后，协方差矩阵P就变成
其中，Pˊ就是原来的误差协方差矩阵（在CTRV模型中就是一个 5×5 的矩阵），Q是处理噪声的协方差矩阵，在CTRV模型中考虑到Q的形式为：
所以计算增广的Sigma点是
即:
且在CTRV模型中，n=7, 所以要取15个sigma点。——同时也意味着是一个7*15的矩阵。
同样的可以代入权重公式求出对应的权重值wi。
STEP2: 计算（2n+1）个 Sigma点集的一步预测，i=1,2,...2n+1。即使用系统方程更新下一时刻（k）所有sigma points的状态。
需要注意的是：这里的输入是一个 (7,15)的矩阵（因为考虑了两个噪声量），但是输出应该修正为一个（5,15）的矩阵。（因为这是预测的结果，本质上是基于运动模型的先验，先验的均值中不应该包含μa、μωˊ 这类不确定的量）
STEP3: 使用无迹变换得到的并且经过状态转移的sigma点计算先验预测均值与协方差。也就是在计算系统状态量的一步预测。也就是先验估计值。
# ----------------------------------------------------------- 更新过程 ------------------------------------------------------
STEP4: 根据一步预测值，再次使用无迹变换，产生新的Sigma点集。
注意下标是 k+1。
STEP5: 根据STEP4得到的新的Sigma点集，代入测量方程 h(x) ，得到预测的测量值。
STEP6: 根据STEP5得到的 Sigma点集的观测预测值，通过加权求和得到系统预测的均值以及协方差。
STEP7: 计算Kalman增益矩阵
STEP8: 计算系统的状态更新和协方差更新
最后放一下KF和UKF的迭代公式的对比：
1.3 UKF的Python仿真代码
# 该代码参考自：https://github.com/raghuramshankar/kalman-filter-localization
# 该代码使用无迹卡尔曼滤波UKF处理非线性运动模型CTRVimport sys
import osimport numpy as npsys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../utils/")import mathimport matplotlib.pyplot as plt
import numpy
import scipy.linalgfrom utils.angle import rot_mat_2d
plt.rcParams['font.sans-serif']=['SimHei'] #用来正常显示中文标签
plt.rcParams['axes.unicode_minus']=False #用来正常显示负号# 协方差矩阵
# 建模噪声的协方差矩阵Q
Q = np.diag([0.1, # X轴位置 的标准差0.1, # Y轴位置 的标准差np.deg2rad(1.0), # 偏转角 的标准差1.0 # 速度的标准差
]) **2# 只观测X、Y坐标的测量噪声的标准差
R = np.diag([1.0, 1.0]) ** 2# 仿真参数
INPUT_NOISE = np.diag([1.0, np.deg2rad(30.0)]) **2
GPS_NOISE = np.diag([0.5, 0.5]) **2DT = 0.1 # 时间间隔 单位 [s]
SIM_TIME = 50.0 # 仿真的时长 单位 [s]# UKF的参数
ALPHA = 0.001
BETA = 2
KAPPA = 0show_animation = True# 初始输入值
def calc_input():v = 1.0 # 初始速度 单位m/syawRate = 0.1 # 初始角速度 单位rad/su = np.array([[v, yawRate]]).Treturn u# 建模方程
def motion_model(x, u):F = np.array([[1.0, 0, 0, 0],[0, 1.0, 0, 0],[0, 0, 1.0, 0],[0, 0, 0, 0]])B = np.array([[DT * math.cos(x[2]), 0],[DT * math.sin(x[2]), 0],[0.0, DT],[1.0, 0.0]])x = F @ x + B @ u# 我自己算了一下,这个建模方程算出来是# X = [# [x + v*cos(偏转角)*dt],# [y + v*sin(偏转角)*dt],# [偏转角 + 角速度*dt],# [速度v]# ]return xdef observation_model(x):H = np.array([[1, 0, 0, 0],[0, 1, 0, 0]])# 没加过噪声z = H @ xreturn zdef observation(xTrue, xd, u):# 真实的状态(无噪)xTrue = motion_model(xTrue, u)# 给GPS的XY坐标数据加入一些高斯白噪声z = observation_model(xTrue) + GPS_NOISE @ np.random.randn(2, 1)# 给输入input加入噪声ud = u + INPUT_NOISE @ np.random.randn(2, 1)# 含有噪声的状态向量空间xd = motion_model(xd, ud)return xTrue, z, xd, ud# 定义生成Sigma点的函数
def generate_sigma_points(xEst, PEst, gamma):# i=0, 第一个sigma点就是输入值sigma = xEst# 对矩阵求平方根Psqrt = scipy.linalg.sqrtm(PEst)n = len(xEst[:, 0])# i=1,2,...,n Positive directionfor i in range(n):sigma = np.hstack((sigma, xEst + gamma * Psqrt[:, i:i+1])) # Psqrt[:, i:i + 1]是Psqrt的第(i+1)列# print('Psqrt[:, i:i+1]',Psqrt[:, i:i+1])# i=n+1,n+2,...2n Negative directionfor i in range(n):sigma = np.hstack((sigma, xEst - gamma * Psqrt[:, i:i+1]))return sigma# 使用运动模型进行Sigma点预测def predict_sigma_motion(sigma, u):"""Sigma Points prediction with motion model使用系统方程更新下一时刻（k）所有sigma points的状态。"""for i in range(sigma.shape[1]): # sigma.shape[1]是sigma的列数sigma[:, i:i+1] = motion_model(sigma[:, i:i+1], u)return sigma# 使用观测模型进行Sigma点预测def predict_sigma_observation(sigma):"""Sigma Points prediction with observation model使用测量方程更新下一时刻（k）所有sigma points的状态。"""for i in range(sigma.shape[1]):# sigma[:, i] 是 sigma矩阵的第 i+1 列# observation_model(sigma[:, i]) 是 sigma矩阵的第 i+1 列经过测量空间,考虑到测量矩阵只有2行# 所以这个结果也是一个 (2*1) 的列向量sigma[0:2, i] = observation_model(sigma[:, i])# 赋值sigma = sigma[0:2, :]return sigma# 在先验预测部分根据Sigma点更新误差协方差矩阵
def calc_sigma_covariance(x, sigma, wc, Pi):# nSigma指的是Sigma的总点数(矩阵的列数)nSigma = sigma.shape[1]d = sigma - x[0:sigma.shape[0]]# 噪声的协方差矩阵PiP = Pifor i in range(nSigma):# 一列一列的迭代# wc是sigma对应的权重P = P + wc[0, i] * d[:, i:i+1] @ d[:, i:i+1].Treturn P# 根据Sigma点集的观测预测值，通过加权求和得到X和Z的协方差矩阵Pxz。
def calc_pxz(sigma, x, z_sigma, zb, wc):# 仍然是Sigma的点数 也即sigma矩阵的列数nSigma = sigma.shape[1]dx = sigma - x# 在后面调用这个函数的时候 这个z_sigma是不加噪声的,将新的sigma点集代入测量方程h(x)后得到的预测的测量值z_sigmadz = z_sigma - zb[0:2]P = np.zeros((dx.shape[0], dz.shape[0]))for i in range(nSigma):P = P + wc[0, i] * dx[:, i:i+1] @ dz[:, i:i+1].Treturn P# -----------------------------UKF的迭代过程----------------------------------
def ukf_estimation(xEst, PEst, z, u, wm, wc, gamma):# 先验预测# STEP1 根据输入的 状态变量xEst 和 误差协方差矩阵PEst 生成sigma点sigma = generate_sigma_points(xEst, PEst, gamma)# STEP2 使用系统方程更新下一时刻（k）所有sigma points的状态sigma = predict_sigma_motion(sigma, u)# STEP3 使用STEP2中的无迹变换得到的并且经过状态转移的sigma点计算先验预测均值xPred 与先验预测协方差矩阵PPredxPred = (wm @ sigma.T).TPPred = calc_sigma_covariance(xPred, sigma, wc, Q)# 时间更新zPred = observation_model(xPred) # 没加噪声,但是映射到测量空间了y = z - zPred # z是输入的什么？？# STEP4 根据一步预测值，再次使用无迹变换，产生新的Sigma点集sigma = generate_sigma_points(xPred, PPred, gamma)# STEP5 根据STEP4得到的新的Sigma点集，代入测量方程 h(x) ，得到预测的测量值z_sigma = predict_sigma_observation(sigma)# STEP6 根据STEP5得到的 Sigma点集的观测预测值，通过加权求和得到系统预测的均值以及协方差zb = (wm @ sigma.T).T # 加权的sigma点集Pzz = calc_sigma_covariance(zb, z_sigma, wc, R) # 我把源代码中的st换成了PzzPxz = calc_pxz(sigma, xPred, z_sigma, zb, wc)# STEP7 计算卡尔曼增益矩阵K = Pxz @ np.linalg.inv(Pzz)# STEP8 计算系统的状态更新和协方差更新xEst = xPred + K @ yPEst = PPred - K @ Pzz @ K.Treturn xEst,PEst# ukf的一些参数
def setup_ukf(nx):# lambda λ的表达公式lamb = ALPHA ** 2 * (nx + KAPPA) -nx# 计算权重# 对于求均值来说,第一个采样点的权重值是wm = [lamb / (lamb + nx)]# 对于求方差来说,第一个采样点的权重值是wc = [(lamb / (lamb + nx)) +(1- ALPHA **2 + BETA)]# 对于剩下的2n个采样点来说 求期望的权值和求方差的权值都相同for i in range(2 * nx):wm.append(1 / (2 * (nx + lamb)))wc.append(1 / (2 * (nx + lamb)))gamma = math.sqrt(nx + lamb)wm = np.array([wm])wc = np.array([wc])return wm,wc,gamma# 画误差协方差椭圆
def plot_covariance_ellipse(xEst, PEst): # pragma: no coverPxy = PEst[0:2, 0:2]eigval, eigvec = np.linalg.eig(Pxy)if eigval[0] >= eigval[1]:bigind = 0smallind = 1else:bigind = 1smallind = 0t = np.arange(0, 2 * math.pi + 0.1, 0.1)a = math.sqrt(eigval[bigind])b = math.sqrt(eigval[smallind])x = [a * math.cos(it) for it in t]y = [b * math.sin(it) for it in t]angle = math.atan2(eigvec[1, bigind], eigvec[0, bigind])fx = rot_mat_2d(angle) @ np.array([x, y])px = np.array(fx[0, :] + xEst[0, 0]).flatten()py = np.array(fx[1, :] + xEst[1, 0]).flatten()plt.plot(px, py, "--r")def main():print(__file__ + "开始！！")nx = 4 # 状态变量 State Vector [x y yaw v]xEst = np.zeros((nx,1))xTrue = np.zeros((nx,1))PEst = np.eye(nx)xDR = np.zeros((nx,1)) # 航迹推算# 计算权重和参数wm, wc, gamma = setup_ukf(nx)# 初始化一下hxEst = xEsthxTrue = xTruehxDR = xTruehz = np.zeros((2,1))htrace = PEst[0,0] + PEst[1,1] + PEst[2,2] + PEst[3,3]time = 0.0while SIM_TIME >= time:time += DTu = calc_input()# xTrue是无噪的真实的状态向量 z是加了高斯白噪声的测量值 xDR是含有噪声的状态向量 ud是带噪声的输入inputxTrue, z, xDR, ud = observation(xTrue, xDR, u)# 后验估计值xEst, PEst = ukf_estimation(xEst, PEst, z, ud, wm, wc, gamma)# 误差协方差矩阵的迹PEst_trace = PEst[0,0] + PEst[1,1] + PEst[2,2] + PEst[3,3]# 存储数据历史hxEst = np.hstack((hxEst, xEst)) # 状态变量的后验估计值hxDR = np.hstack((hxDR, xDR)) # 航迹推算值hxTrue = np.hstack((hxTrue, xTrue)) # 状态空间的真实值(无噪)hz = np.hstack((hz, z)) # 含噪的测量值htrace = np.hstack((htrace, PEst_trace)) # 误差协方差的迹if show_animation:plt.cla()# 用ESC停止仿真过程plt.gcf().canvas.mpl_connect('key_release_event',lambda event: [exit(0) if event.key == 'escape' else None])plt.plot(hz[0, :], hz[1, :], ".g") # 含噪的测量值 绿色点plt.plot(np.array(hxTrue[0, :]).flatten(),np.array(hxTrue[1, :]).flatten(), "-b" ) # 不含噪声的真实轨迹 蓝色线图plt.plot(np.array(hxDR[0, :]).flatten(),np.array(hxDR[1, :]).flatten(), "-k") # 航迹推算值 黑色线图plt.plot(np.array(hxEst[0, :]).flatten(),np.array(hxEst[1, :]).flatten(),"-r") # 后验估计值 红色线图# plt.plot(np.array(htrace).flatten(), "-y") # 误差协方差的迹 黄色线图# plot_covariance_ellipse(xEst, PEst)plt.title("UKF in CTRV模型")plt.axis("equal")plt.xlabel('X坐标 单位:m')plt.ylabel('Y坐标 单位:m')plt.grid(True)plt.pause(0.001)if __name__ == '__main__' :main()1.4 EKF和UKF的实验结果图对比
初始速度设置：
v = 1.0 # [m/s] yawrate = 0.1 # [rad/s]
误差协方差矩阵设置：
Q = np.diag([0.1, # variance of location on x-axis0.1, # variance of location on y-axisnp.deg2rad(1.0), # variance of yaw angle1.0 # variance of velocity ]) ** 2 # predict state covariance R = np.diag([1.0, 1.0]) ** 2 # Observation x,y position cvariance
输入误差设置：
INPUT_NOISE = np.diag([1.0, np.deg2rad(30.0)]) **2 GPS_NOISE = np.diag([0.5, 0.5]) **2d
（1）当仿真时长为50s时：
EKF:误差协方差矩阵的迹是 5.13443600949823
UKF:误差协方差的迹是 1.23580285643872
（2）当仿真时长为60s时：
EKF:误差协方差的迹是 5.14080308937101
UKF:误差协方差的迹是 1.2247423745773207
（3）当仿真时长为100s时：
EKF:误差协方差的迹是 5.150677131877716
UKF:误差协方差的迹是 1.2266064534767611
足以见得UKF的滤波结果优于EKF。
1.5 UKF参数修改后的实验结果
起因是看到这篇博客讲的：TSLAM9--------中心差分卡尔曼滤波（cdkf）_L菌的小跟班的博客-CSDN博客_中心差分卡尔曼
很巧，我的仿真实验的参数设置正好是：
# UKF的参数 ALPHA = 0.001 BETA = 2 KAPPA = 0
所以去测了一下改改参数后的误差协方差矩阵的迹会有什么变化：
1.5.1 逐步调大ALPHA
实验1：
UKF
100S
ALPHA = 0.01
BETA = 2
KAPPA = 0
误差协方差的迹是 1.2278020934497518
实验2：
UKF
60S
ALPHA = 1
BETA = 2
KAPPA = 0
误差协方差的迹是 1.2213499062975814
实验3：
UKF
60S
ALPHA = 2
BETA = 2
KAPPA = 0
误差协方差的迹是 1.8477693356710194
ALPHA到了2之后精度下降很明显，于是回过头:
实验4：
UKF
60S
ALPHA = 0.9
BETA = 2
KAPPA = 0
误差协方差的迹是 1.2238780165190908
ALPHA = 0.8
误差协方差的迹是 1.2268426409596127
ALPHA = 0.7
误差协方差的迹是 1.2193049965501062
ALPHA = 0.6
误差协方差的迹是 1.2261196156044387
ALPHA = 0.5
误差协方差的迹是 1.2219465177621889
ALPHA = 0.4
误差协方差的迹是 1.2277252949548005
ALPHA = 0.3
误差协方差的迹是 1.2188162232049613
ALPHA = 0.2
误差协方差的迹是 1.2229605401101256
ALPHA=0.1
误差协方差的迹是 1.2247298134377802
实验5：
UKF
60S
ALPHA = 1.1
BETA = 2
KAPPA = 0
误差协方差的迹是 1.2273251200853337
经过实验，ALPHA = 1.6及更大之后，误差协方差矩阵的迹明显增加至2左右。
滤波笔记三：无迹卡尔曼滤波（UKF）相关推荐
- 无迹卡尔曼滤波UKF—目标跟踪中的应用(算法部分)
无迹卡尔曼滤波UKF-目标跟踪中的应用(算法部分) 原创不易,路过的各位大佬请点个赞 机动目标跟踪/非线性滤波/传感器融合/导航等探讨代码联系WX: ZB823618313 仿真部分见博客: [无迹卡 ...
- 无迹卡尔曼滤波UKF—目标跟踪中的应用(仿真部分)
无迹卡尔曼滤波UKF-目标跟踪中的应用(仿真部分) 原创不易,路过的各位大佬请点个赞 机动目标跟踪/非线性滤波/传感器融合/导航等探讨联系WX: ZB823618313 算法部分见博客: [无迹卡尔曼 ...
- 对无迹卡尔曼滤波(UKF)和粒子滤波(PF)的理解
对无迹卡尔曼滤波(UKF)的总结: EKF通过在工作点附近进行一阶泰勒展开来线性化非线性的运动方程和观测方程.这样的线性近似在高度非线性的情况下存在较大误差.而UKF换了一个线性化的思路.1)选取2n ...
- 无迹卡尔曼滤波UKF和容积卡尔曼滤波CKF进行锂电池SOC估计的C语言版本实现，和matlab版本一样包含定参和FFRLS两种情况
(C语言版)无迹卡尔曼滤波UKF和容积卡尔曼滤波CKF进行锂电池SOC估计的C语言版本实现,和matlab版本一样包含定参和FFRLS两种情况,已在VS2019和Ubuntu 20.04.4版本中运行 ...
- 无迹卡尔曼滤波UKF和容积卡尔曼滤波CKF进行锂电池SOC估计的C语言版本实现
无迹卡尔曼滤波UKF和容积卡尔曼滤波CKF进行锂电池SOC估计的C语言版本实现,和matlab版本一样包含定参和FFRLS两种情况 (C语言版)无迹卡尔曼滤波UKF和容积卡尔曼滤波CKF进行锂电池SO ...
- （C语言版）无迹卡尔曼滤波UKF和容积卡尔曼滤波CKF进行锂电池SOC估计的C语言版本实现
(C语言版)无迹卡尔曼滤波UKF和容积卡尔曼滤波CKF进行锂电池SOC估计的C语言版本实现,和matlab版本一样包含定参和FFRLS两种情况,已在VS2019和Ubuntu 20.04.4版本中运行 ...
- ukf实测信号的预测 matlab,无迹卡尔曼滤波(UKF)在参数估计应用中迭代停滞问题
本帖最后由 华仔zju 于 2017-3-23 16:40 编辑 最近项目上在利用卡尔曼滤波进行模型参数估计,编写的EKF程序运行良好,后来看到论文提及UKF效果会比EKF更好,便想着使用UKF试一试 ...
- 滤波笔记一：卡尔曼滤波（Kalman Filtering）详解
本笔记是总结了B站DR_CAN的卡尔曼滤波器的课程,他的B站主页为:DR_CAN的个人空间_哔哩哔哩_bilibili PS:虽然我不是学自控的,但是老师真的讲的很好! 目录 Lesson1 递归算法 ...
- 滤波笔记二：运动模型（CVCACTRV）
写这篇文章是因为在学习卡尔曼滤波的时候发现,只有线性运动可以用卡尔曼滤波,而非线性运动需要用到扩展卡尔曼滤波(EKF)或者无迹卡尔曼滤波(UKF).那么又发现自己不熟悉非线性运动的运动模型,所以学了一 ...
最新文章
- Leetcode 4.28 Tree Easy
- 第一章 初识Mysql
- 预处理_气体在线分析仪预处理系统的工作内容
- python中counter_Python collections模块中counter（）的详细说明,Pythoncollections,之,Counter,详解...
- 一分钟教你用Excel从统计局抓数据！
- BTrace：Java开发人员工具箱中的隐藏宝石
- memcached 缓存 分布式缓存 常见命令,管理命令
- ClickHouse数据导入(Flink、Spark、Kafka、MySQL、Hive)
- 《Unity3D脚本编程与游戏开发》学习Day one
- 【科研作图】软件推荐资源汇总PPT瑞思拜！
- 任正非对姚安娜是劝退式支持吗
- 804计算机考研,北京邮电大学804信号系统考研经验
- 怎样写好git comment
- Mac pip confirming SSL certificate
- php完美导出word,php导出生成word的方法
- KeyShot 实时光线追踪三维渲染软件
- matlab 曲线填充 透明度
- JS - 实现word预览功能
- 聚美自建的“真品联盟”被京东捅破的窗户纸
- IC基础知识（八）ROM、PROM、EPROM、EEPROM和Flash之间的区别