卡尔曼滤波

以下介绍部分乃网络资料，程序注释和优化乃自己原创： Dr Kalman 的卡尔曼滤波器。下面的 描述 ，会涉及一些基本的概念知识，包括概率（Probability），随机变量（Random Variable），高斯或正态分配（Gaussian Distribution）还有State-space Model等等。但对于卡尔曼滤波器的详细证明，这里不能一一描述。 　　首先，我们先要引入一个离散控制过程的 系统 。该系统可用一个线性随机微分方程（Linear Stochastic Difference equation）来描述： 　　X(k)=A X(k-1)+B U(k)+W(k) 　　再加上系统的测量值： 　　Z(k)=H X(k)+V(k) 　　上两式子中，X(k)是k时刻的系统状态，U(k)是k时刻对系统的控制量。A和B是系统参数，对于多模型系统，他们为矩阵。Z(k)是k时刻的测量值，H是测量系统的参数，对于多测量系统，H为矩阵。W(k)和V(k)分别表示过程和测量的噪声。他们被假设成高斯白噪声(White Gaussian Noise)，他们的COVariance 分别是Q，R（这里我们假设他们不随系统状态变化而变化）。 　　对于满足上面的条件(线性随机微分系统，过程和测量都是高斯白噪声)，卡尔曼滤波器是最优的信息处理器。下面我们来用他们结合他们的covariances

参考： https://en.wikipedia.org/wiki/Kalman_filter 原文： https://www.cnblogs.com/alantu2018/p/9224001.html 公式： 首先，我们先要引入一个离散控制过程的系统。该 系统可用一个线性随机微分方程（ Linear Stochastic Difference equation ）来描述： X(k)=A X(k-1)+B U(k)+W(k) 再加上系统的测量值： Z(k)=H X(k)+V(k) 上两式子中 ： X(k) 是 k 时刻的系统状态 ； U(k) 是 k 时刻对系统的控制量 ； A 和 B 是系统参数，对于多模型系统，他们为矩阵 ； Z(k) 是 k 时刻的测量值 ； H 是测量系统的参数，对于多测量系统， H 为矩阵 ； W(k) 和 V(k) 分别表示过程和测量的噪声。他们被假设成高斯白噪声 (White Gaussian Noise) ，他们的 covariance 分别是 Q ， R （这里我们假设他们不随系统状态变化而变化）。 对于满足上面的条件 ( 线性随机微分系统，过程和测量都是高斯白噪声 ) ，卡尔曼滤波器是最优的信息处理器。 下面我们来用他们结合他们的 covariances 来估算系统的最优化输出（类似上一节那个温度的例子）。 首先我们要利用系统的过程模型

简介 　　已经历经了半个世纪的卡尔曼滤波至今仍然是研究的热点，相关的文章不断被发表。其中许多文章是关于卡尔曼滤波器的新应用，但也不乏改善和扩展滤波器算法的研究。而对算法的研究多着重于将卡尔曼滤波应用于非线性系统。 　　为什么学界要这么热衷于将卡尔曼滤波器用于非线性系统呢？因为卡尔曼滤波器从一开始就是为线性系统设计的算法，不能用于非线性系统中。但是事实上多数系统都是非线性的，所以如果卡尔曼滤波器不能用在非线性系统中的话，那么它的应用范围就非常有限了。如果真的是这样，卡尔曼滤波器可能早就寿终正寝或者过很久很久才会被人注意到。幸运的是早期的学者们对这个问题理解的非常深刻，而且也找到了解决方法，就是扩展卡尔曼滤波（EKF）。 　　事实上世界上的第一个卡尔曼滤波也是扩展卡尔曼滤波，而不是线性卡尔曼滤波器。扩展卡尔曼滤波有很久远的历史，如果说有一个 非线性系统 需要用到卡尔曼滤波的话，不必怀疑，先试试扩展卡尔曼滤波准没错。因为他有很久远的历史，所以可以轻松的找到许多这方面的资料。 　　不过扩展卡尔曼滤波也不是无懈可击的，它有一个很 严重的短板——发散 。使用扩展卡尔曼滤波的时候请务必记在心上，时刻提醒自己，这样设计滤波器其结果会发散吗？毫不夸张地说相对于线性卡尔曼滤波设计扩展卡尔曼滤波器的就是在解决发散问题。发散问题解决了剩下的都是小事。 小结： 扩展卡尔曼滤波器主要用于非线性系统；

说明 : 本项目基于BOT模型的平面目标跟踪，比较了几种典型的非线性滤波算法，包括滤波算法估计精度、 计算量、稳定性、计算时间等非常重要的滤波算法性能，得出了一些结论，供大家参考。 关键词 : 非线性卡尔曼滤波 EKF UKF CKF ECKF FCKF 蒙特卡洛打靶 均方根误差(RMSE) 参考文献: [1] The High-Degree Cubature Kalman Filter (Bin Jia, Ming Xin, and Yang Cheng) [2] 基于正交变换的五阶容积卡尔曼滤波导航算法 (何康辉，董朝阳 ) [3] 高阶容积卡尔曼滤波及其在目标跟从中的应用 [4] 五阶容积卡尔曼滤波算法及其应用 [5] 广义容积卡尔曼滤波 [6] 均方根嵌入式容积卡尔曼滤波 [7] EKF_UKF和CKF的滤波性能对比研究_常宇健 [8] Unscented Kalman Filter Tutorial (Gabriel A. Terejanu ) [9] INSGNSS深组合导航系统的非线性研究 [10] 基于惯导视觉组合的无人机相对导航方法研究 [11] 面向目标跟踪的非线性滤波算法性能分析研究 主函数main : % % 基于BOT模型的非线性滤波算法比较 % % 参考文献 : % [ 1 来源： CSDN 作者： Aircraft GNC 链接： https:/

% HCKF高阶容积卡尔曼滤波在自动驾驶车辆定位中的应用 % 参考文献 : % The High - Degree Cubature Kalman Filter ( Bin Jia , Ming Xin , and Yang Cheng ) % Unscented Kalman Filter Tutorial ( Gabriel A . Terejanu ) % 基于正交变换的五阶容积卡尔曼滤波导航算法 ( 何康辉，董朝阳 ) % ＥＫＦ、ＵＫＦ和ＣＫＦ的滤波性能对比研究 ( 常宇健，赵辰 ) % 组合导航原理与应用 ( 西北工业大学出版社 ) % 卡尔曼滤波与Matlab仿真 ( 北京航空航天大学出版社 ) % 本例子使用线性状态模型，因此加速度变成了干扰项 % Lidar传感器测量模型为线性模型，Radar传感器测模型为非线性模型 % 在Radar传感器中使用滤波算然为 HCKF , 在Lidar传感器中使用滤波算然为 KF clear ; clc ; tic ; % 计时 dt = 0.1 ; % 步长 % 分配空间 Radar_measurement = [ ] ; Radar_measurement_p = [ ] ; Lidar_measurement = [ ] ; HCKF_estimation = [ ] ; hckf = [ ] ; % 结构体 %

图解卡尔曼滤波 来源： CSDN 作者： weixin_39566143 链接： https://blog.csdn.net/weixin_39566143/article/details/104653150

与我的朋友交谈时，我经常听到：“哦，卡尔曼(Kalman)滤波器……我经常学它，然后我什么都忘了”。好吧，考虑到卡尔曼滤波器(KF)是世界上应用最广泛的算法之一(如果环顾四周，你80％的技术可能已经在内部运行某种KF)，让我们尝试将其弄清楚。 在这篇文章的结尾，你将对KF的工作原理，其背后的想法，为什么需要多个变体以及最常见的变体有一个直观而详细的了解。 状态估计 KF是所谓的状态估计算法的一部分。什么是状态估计？假设你有一个系统(让我们将其视为黑箱)。黑箱可以是任何东西：你的风扇，化学系统，移动机器人。对于这些系统中的每一个，我们都可以定义一个状态。状态是我们关心的变量向量，可以描述系统处于特定时间点的“状态”(这就是为什么将其称为状态)。“可以描述”是什么意思？这意味着，如果你了解当时的状态向量k和提供给系统的输入，则可以了解当时的k+1的系统状态(与此同时使用系统工作原理的一些知识)。 例如，假设我们有一个移动的机器人，并且我们关心其在空间中的位置(并且不在乎其方向)。如果我们将状态定义为机器人的位置(x, y)及其速度，( v x v_x v x ​ , v y v_y v y ​ )并且我们有一个机器人如何运动的模型，那么就足以确定机器人的位置以及下一个时刻的位置。 因此，状态估计算法估计系统的状态。为什么要估算呢？因为在现实生活中，外部观察者永远无法访问系统的真实状态

UKF UKF为Unscented Kalman Filter简写，中文可翻译为无损卡尔曼滤波，本滤波是为了解决非线性问题存在的（也可用于线性模型），即结合了UT变换和标准卡尔曼滤波的一种方法。 UKF原理的通俗解释 其实对于状态的一步递推和量测的一步递推，是不存在非线性带来的难题的。考虑以下非线性模型： x k + 1 = f ( x k ) + w x_{k+1}=f(x_{k})+w x k + 1 ​ = f ( x k ​ ) + w y k + 1 = h ( x k ) + v y_{k+1}=h(x_{k})+v y k + 1 ​ = h ( x k ​ ) + v 可以很简单的通过方程 f ( x ) f(x) f ( x ) 和 h ( x ) h(x) h ( x ) 求得一步递推值。但是，对于误差特性的传递却不能通过非线性方程求取了。 例如，对于线性方程来说，误差的传递也是线性的，非线性模型却比较麻烦。EKF的思路是将模型线性化，还是通过经典卡尔曼滤波的思路解决问题。UKF另辟蹊径，通多采样的方法，将采样点通多非线性模型一一传递，然后计算传递后的采样点映射点的统计误差！ 于是一切有解了。 一个很简单的模型 下边是我们此次试验的模型，首先它是一维的，易于直观理解其原理，另外可以发现此模型的状态方程式线性的，观测方程是非线性的，取此测试模型是刻意的

线性卡尔曼滤波KF clear; clc; N=100; Z=(1:N);%观测值 noise=randn(1,N);%mean=0,方差为1的高斯噪声 Z=Z+noise; X=[0;0];%初始状态 P=[1 0; 0 1];%状态协方差矩阵 F=[1 1; 0 1];%状态转移矩阵 Q=[0.0001 0; 0 0.0001];%状态转移协方差矩阵0.0001 0; 0 0.0001 H=[1 0];%观测矩阵 R=1;%观测噪声方差 figure; hold on; sz = [2,N]; res=zeros(sz); for i=1:N X_=F*X; P_=F*P*F'+Q; K=P_*H'/(H*P_*H'+R); X=X_+K*(Z(i)-H*X_); P=(eye(2)-K*H)*P_; res(1,i) = X(1); res(2,i) = X(2); %plot(X(1), X(2),'r-');%横轴表示位置&纵轴表示速度 end plot(res(1,:), res(2,:),'r-+'); 来源： CSDN 作者： hjwang1 链接： https://blog.csdn.net/hjwang1/article/details/103647852

学习卡尔曼滤波看了4天的文章,硬是没看懂.后来找到了下面的文章一下就看懂了. 我对卡尔曼滤波的理解, 我认为,卡尔曼滤波就是把统计学应用到了滤波算法上. 算法的核心思想是 ,根据当前的仪器"测量值" 和上一刻的 "预测量" 和 "误差",计算得到当前的最优量. 再 预测下一刻的量, 里面比较突出的是观点是. 把误差纳入计算, 而且分为预测误差和测量误差两种.通称为 噪声. 还有一个非常大的特点是,误差独立存在, 始终 不受测量数据的影响. 上面的ppt有助于入门理解. 但是在编程的时候你会发现,解释里面的数值23 没有很明确的指出,是指的那个时刻的23 是预测的23 还是上一课测量的23 下面这段文字会有助于你更清晰的理解 卡尔曼滤波是统计学的程序表达. 要想深入理解,公式三 协方差的背后意义 需要学习统计学. 如果仅仅是使用的话,这5个公式套进程序里面还是很容易的. 看到这里如果你明白了原理, 你再回过头看看,会发现.误差是独立存在的. 误差不受数据的影响. 误差按照统计学的协方差公式更新, 跟数据无关. 而且误差是不断变化的. 来源： CSDN 作者： peanut_wu 链接： https://blog.csdn.net/laziji/article/details/104027425