容积卡尔曼滤波CKF—目标跟踪中的应用(算法部分—II)

原创不易，路过的各位大佬请点个赞

机动目标跟踪/非线性滤波/传感器融合/导航等探讨联系WX: ZB823618313

作者:823618313@qq.com
备注：
容积卡尔曼滤波算法；CKF；Cubature Kalman Filter
两种CKF算法：加性噪声CKF和非加性噪声CKF
本博客主要讲解“非加性噪声条件下的容积卡尔曼滤波算法”推导结果
matlab实现;
目标跟踪仿真
Case: 二维目标跟踪情况和三维目标跟踪情况
代码下载地址如下（分别为二维情形和三维情形）

容积卡尔曼滤波2D
https://download.csdn.net/download/weixin_44044161/85401361

容积卡尔曼滤波2D——CT模型
https://download.csdn.net/download/weixin_44044161/85401924

容积卡尔曼滤波2D——RMSE
https://download.csdn.net/download/weixin_44044161/85124145

容积卡尔曼滤波3D——RMSE
https://download.csdn.net/download/weixin_44044161/85124127

无迹卡尔曼滤波UKF—目标跟踪中的应用

容积卡尔曼滤波CKF—目标跟踪中的应用(算法部分—II)
- 1、带非加性噪声的容积卡尔曼滤波算法CKF
- - 1.1 问题描述（离散时间非线性系统描述）
  - 1.2 容积点集
  - 1.3 非加性噪声容积卡尔曼滤波CKF算法
- 2. 仿真实验
- - 4.1 主函数

容积卡尔曼滤波思考：
为了克服无迹卡尔曼滤波在高维情况下出现滤波精度低的问题，Arasaratnam 和Haykin基于Caubature求积分变换，提出了容积卡尔曼滤波CKF方法。后来众多学者又基于CKF，提出了很多改进版本，如平方根CKF。
对于高斯分布下的非线性滤波问题，实际上就求后验期望的积分。由于被积分函数表现为非线性后验分布与高斯概率密度的乘积，因此一般很难得到解析解。这也是线性系统下该积分可以得到解析解，即著名的卡尔曼滤波算法。
E[x∣z]=∫Rf(x)exp⁡(−xTx)dxE[x|z]=\int_Rf(x)\exp(-x^Tx)dxE[x∣z]=∫Rf(x)exp(−xTx)dx
因此针对该非线性函数的积分问题，营运产生了众多基于数值积分的滤波算法。如UKF通过确定性采样来传播分布的一二阶矩（均值和方差）。而CKF作为看另一种求积分近似方法，利用球面径向规则。

CKF和UKF 总结：
当取κ=0\kappa=0κ=0时， CKF 和 UKF 的估计精度相同，但鉴于 CKF 采样点少，实时性
比 UKF 好，故应选用 CKF 滤波算法；
当n≤2n\leq2n≤2时即低维非线性系统， UKF 的估计精度高于 CKF，应选用 UKF 滤波
算法；
当n=2n=2n=2时的非线性系统， UKF 及 CKF 的估计精度相同，但 CKF 的实时性更
好，应选用 CKF 滤波算法；
当 n≥3n\geq3n≥3时即高维非线性系统， CKF 的估计精度高于 UKF，应选用 CKF 滤波算法。

下面介绍加性噪声下的容积卡尔曼滤波算法

1、带非加性噪声的容积卡尔曼滤波算法CKF

1.1 问题描述（离散时间非线性系统描述）

考虑带非加性噪声的一般非线性系统模型，
xk=f(xk−1，wk−1)zk=h(xk，vk)(2-1)x_k=f(x_{k-1}， w_{k-1}) \\ z_k=h(x_k， v_k) \tag{2-1}xk=f(xk−1，wk−1)zk=h(xk，vk)(2-1)
其中xkx_kxk为kkk时刻的目标状态向量。zkz_kzk为kkk时刻量测向量（传感器数据）。这里不考虑控制器uku_kuk。wk{w_k}wk和vk{v_k}vk分别是过程噪声序列和量测噪声序列，并假设wkw_kwk和vkv_kvk为零均值高斯白噪声，其方差分别为QkQ_kQk和RkR_kRk的高斯白噪声，即wk∼(0,Qk)w_k\sim(0,Q_k)wk∼(0,Qk), vk∼(0,Rk)v_k\sim(0,R_k)vk∼(0,Rk)，且满足如下关系(线性高斯假设)为：
E[wivj′]=0E[wiwj′]=0i≠jE[vivj′]=0i≠j\begin{aligned} E[w_iv_j'] &=0\\ E[w_iw_j'] &=0\quad i\neq j \\ E[v_iv_j'] &=0\quad i\neq j \end{aligned} E[wivj′]E[wiwj′]E[vivj′]=0=0i=j=0i=j

1.2 容积点集

{ξi,wi}\{\xi_i, w_i\}{ξi,wi}
利用三阶球面径向规则计算标准高斯加权积分，即
E[x∣z]=∫Rf(x)N(x;0,I)dx≈∑i=1mwif(ξi)E[x|z]=\int_Rf(x)\mathcal{N}(x; 0,I)dx\approx\sum_{i=1}^{m}w_if(\xi_i)E[x∣z]=∫Rf(x)N(x;0,I)dx≈i=1∑mwif(ξi)
式中
ξi=m2[1]i,i=1,2,⋯,m=2nwi=1m,i=1,2,⋯,m=2n(2)\begin{aligned} \xi_i&=\sqrt{\frac{m}{2}}[\mathbf{1}]_i, i=1,2,\cdots,m=2n\\ w_i&=\frac{1}{m}, i=1,2,\cdots,m=2n \end{aligned} \tag{2} ξiwi=2m[1]i,i=1,2,⋯,m=2n=m1,i=1,2,⋯,m=2n(2)
[1][\mathbf{1}][1] 表示nnn（nnn维状态维数）维空间的点集，即

1.3 非加性噪声容积卡尔曼滤波CKF算法

2. 仿真实验

容积卡尔曼滤波2D
https://download.csdn.net/download/weixin_44044161/85401361

容积卡尔曼滤波2D——CT模型
https://download.csdn.net/download/weixin_44044161/85401924

容积卡尔曼滤波2D——RMSE
https://download.csdn.net/download/weixin_44044161/85124145

容积卡尔曼滤波3D——RMSE
https://download.csdn.net/download/weixin_44044161/85124127

以二维雷达目标跟踪实列做为仿真案列。
说明:
1.二维仿真代码也可以在上面的连接中直接下载，
2.将UKF函数保存，文件名“fun_2CKF.m”
3.将量测函数保存，文件名“measurements.m”
4. 运行下面的主函数
5. 注意将这三个文件保存在一个文件夹下

4.1 主函数

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% created by:
% date: 2020/4
% 容积卡尔曼滤波，目标跟踪
% 二维目标跟踪问题
% 线性CV目标模型
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear all; close all; clc;
%% initial parameter
n=4; %状态维数 ;
T=1; %采样时间
M=1; %雷达数目
N=200; %运行总时刻
MC=100; %蒙特卡洛次数
chan=1; %滤波器通道，这里只有一个滤波器
w_mu=[0,0]'; % mean of process noise
v_mu=[0,0]'; % mean of measurement noise
%% target model
%covariance of process noise
q_x=0.01; %m/s^2
q_y=q_x;
Qk=diag([q_x^2,q_y^2]);
% state matrix

容积卡尔曼滤波CKF—目标跟踪中的应用(仿真部分—II)相关推荐

容积卡尔曼滤波CKF—目标跟踪中的应用(算法部分—I)
容积卡尔曼滤波CKF-目标跟踪中的应用(算法部分) 原创不易,路过的各位大佬请点个赞机动目标跟踪/非线性滤波/传感器融合/导航等探讨代码联系WX: ZB823618313 作者:823618313@ ...
无迹卡尔曼滤波UKF—目标跟踪中的应用(仿真部分)
无迹卡尔曼滤波UKF-目标跟踪中的应用(仿真部分) 原创不易,路过的各位大佬请点个赞机动目标跟踪/非线性滤波/传感器融合/导航等探讨联系WX: ZB823618313 算法部分见博客: [无迹卡尔曼 ...
无迹卡尔曼滤波UKF—目标跟踪中的应用(算法部分)
无迹卡尔曼滤波UKF-目标跟踪中的应用(算法部分) 原创不易,路过的各位大佬请点个赞机动目标跟踪/非线性滤波/传感器融合/导航等探讨代码联系WX: ZB823618313 仿真部分见博客: [无迹卡 ...
【卡尔曼滤波】卡尔曼滤波在雷达目标跟踪中的应用仿真matlab源码
1 模型 [摘要]目标跟踪问题的应用背景是雷达数据处理,即雷达在搜索到目标并记录目标的位置数据,对测量到的目标位置数据(称为点迹)进行处理,自动形成航迹,并对目标在下一时刻的位置进行预测.本文简要讨论 ...
MATLAB应用实战系列（七十六）-【仿真应用】卡尔曼滤波在雷达目标跟踪中的应用仿真（附matlab代码）
1 模型目标跟踪问题的应用背景是雷达数据处理,即雷达在搜索到目标并记录目标的位置数据,对测量到的目标位置数据(称为点迹)进行处理,自动形成航迹,并对目标在下一时刻的位置进行预测.本文简要讨论了用Ka ...
交互式多模型-扩展卡尔曼滤波IMM-EKF——机动目标跟踪中的应用
交互式多模型-扩展卡尔曼滤波IMM-EKF--机动目标跟踪中的应用原创不易,路过的各位大佬请点个赞针对机动目标跟踪的探讨.技术支持欢迎联系,也可以站内私信 WX: ZB823618313 机动目标 ...
无迹卡尔曼滤波（UKF）在单观测站目标跟踪中的应用
假定目标做匀速直线运动,在单个观测站对目标进行观测的前提下,再假设目标的初始状态已知.目标的运动方程可以写成如下形式: 其中: 设采样时间间隔T=1s,运行时间为N=60s,W(k)的均方差为,为一个 ...
交互式多模型算法IMM——机动目标跟踪中的应用
机动目标跟踪--交互式多模型算法IMM 原创不易,路过的各位大佬请点个赞 WX: ZB823618313 机动目标跟踪--交互式多模型算法IMM 机动目标跟踪--交互式多模型算法IMM 1. 对机动目 ...
交互式多模型-粒子滤波IMM-PF—在机动目标跟踪中的应用/matlab实现
交互式多模型-粒子滤波IMM-PF-在机动目标跟踪中的应用/matlab实现原创不易,路过的各位大佬请点个赞 WX: ZB823618313 交互式多模型-粒子滤波IMM-PF-在机动目标跟踪中的应 ...

容积卡尔曼滤波CKF—目标跟踪中的应用(仿真部分—II)