weixin_38898944的博客

水面上有条船在做匀速直线航行，航行过程中由于风和浪的影响，会有些随机的干扰，也就是会有些随机的加速度作用在船身上，这个随机加速度的均方差大约是0.1，也就是说方差是0.01。船上搭载GPS设备，能够给出船的位置信息，目前GPS的定位误差为10m，也就它的定位方差是100。船的初始坐标为（-100， 200），水平两个方向的初始速度为（2, 20）。用kalman滤波器对船的航行位置等信息进行滤波估计。

红线是滤波误差，也就是kf.GetX()(0)-X(0)，效果不错吧。也就是说，初始位置95的方差10，初始速度1的方差为1。另外，本来g应该等于9.8的，但是咱们只是仿真，不必较真，程序里的g就写成1了。然后是X的初值，我们让这个物体的初始位置在95米的高度，且有一个向上的初始速度1。状态方程有了，这还是个带控制量的。X(0)是物体位置的真实值，kf.GetX()(0)是滤波结果，Z(0)是位置观测结果。一个物体在重力场下作直线自由落体运动，而观测装置能够观测到这个物体的实时位置，观测的误差是1。

先设计一个kalman滤波器的类。我的矩阵类里已经实现了矩阵的各类运算，包括矩阵加减乘，矩阵与标量的加减乘以及矩阵转置、矩阵求逆、矩阵元素开根号。主程序在运行kalman的同时也对室内温度实际值进行了仿真，并将温度实际值（X），kalman滤波值（kf.GetX()），和量测值（Z）保存到文件里。很多同学看了我之前的文章，都对kalman滤波器的原理有了理解，但我发现，在具体工程设计过程中，还是很多人都感觉到无从下手，一些参数也不知道如何选取。这是一个非常简单的案例，过程中所有的矩阵都是一维，也就是标量。

UDUT分解法，跟三角分解法有异曲同工之妙。那么相对应的也就必然产生一种新的滤波算法，也就是UDUT分解滤波算法。

关于面向对象的一些概念和细节

收集了一些预处理、程序版式、命名规则相关的细节和建议。

C++沙海拾遗值数组、指针和复合数据类型

C和C++语言中的一些基本概念，易混淆的概念和编程建议。

C++和C中的一些基础知识点提炼。

目录最优线性平滑固定点平滑最优线性平滑       前面一系列文章，我觉得我已经基本说明白了一件事，卡尔曼滤波是一种最优估计算法。这回，我们先重新回到最优估计的个概念上来。其实最优估计还可以大致分成三种，滤波、预测和平滑。这三个的概念理解起来不难。       滤波——就是根据1到k时刻的量测Z1Z_1Z1​ ~ ZkZ_kZk​来对k时刻的状态XkX_kXk​进

目录常增益次优滤波上一篇讲了卡尔曼滤波器延伸出来的次优滤波器中的一种手段，降阶处理。这回，我就再介绍一些次优滤波器的其他思路，常增益次优滤波和解耦次优滤波。讽刺的是，我的文章虽然把降阶处理放在前面详细讲解了一下，但降阶处理在工程上却用得不多，反而是这篇文章要讲的这两种方法更具有工程应用价值。常增益次优滤波看过前面的文章的朋友应该都了解了，卡尔曼滤波起到关键作用的参数就是那个K，也就是滤波增益，如果设计的合适，滤波器是稳定的。尤其是对于定常系统（也就是状态方程和量测方程里的系数Φk,k−1\Phi_{k

目录1、降阶滤波的推导思路推导过程实例分析小结1、降阶滤波的推导思路       所谓卡尔曼滤波的降阶处理，就是想办法把状态量的数量少，当然这是有条件的。减少就意味着要忽略一些量，但那些量可以忽略是值得研究的。最直观的想法是忽略我们不关心的量，而且忽略后还得对滤波不会造成太大的影响。什么量可以忽略呢？当然是噪声。       还记得前面提到的有色噪声处理问题吧。

目录一. 序贯处理二. 平方根滤波上一篇文章，我们提到了卡尔曼滤波的发散问题，并说明了，导致卡尔曼滤波发散的原因一般有两种，一种是由于模型建立不够准确，导致的发散，另一种是计算过程由于累计误差效应，导致增益矩阵P失去对称性或正定性而造成的发散。而且，上一篇文章对第一种情况提出了一些解决措施，主要思路就是认为近期的观测量更可靠一些，并逐渐忘记之间的观测对滤波的影响，也就是衰减记忆法和限定记忆法。在这篇文章，我们讨论针对滤波器发散另一种原因可以采取的抑制措施。这里先介绍一种，也就是平方根滤波。不过在介绍这

目录一、卡尔曼滤波发散了怎么办1、衰减记忆法2、限定记忆法一、卡尔曼滤波发散了怎么办几乎任何递推和迭代算法都有发散的可能，卡尔曼滤波也不例外。造成卡尔曼滤波发散的原因无外乎以下两点：状态方程描述的动力学模型不准确，或者噪声的统计模型不准确，这样会使模型和量测值不匹配，导致发散。其实模型不准确的情况很少，除非原理上错误。更多的是由于模型太过粗糙而导致的。递推过程计算机摄入误差累积，使得方差阵失去正定性或者失去对称性，导致增益计算失去加权效果，从而导致滤波器发散。先说模型不准确导致的发散处理技术，累积

目录1、白噪声条件下的卡尔曼滤波器的一般形式2、有色噪声下的卡尔曼滤波系统噪声为有色噪声时的情况1、白噪声条件下的卡尔曼滤波器的一般形式前面的文章介绍了卡尔曼滤波的基本方程和一些简单的应用实例。其实，只有基本方程已经能够适用于很多场合了，工程上用的最多的也就是这个基本方程而已。那为什么还会有一般形式能。这个问题的来源是系统状态方程的一般化形式。我们已经知道，之前介绍的卡尔曼滤波所用到的状态方程只有状态转移和噪声驱动。而很多系统出了这些，它其实还有一项，就是控制项，忘了的同学自己去查控制理论。所以状态

目录实例一、线性定常系统实例一、线性定常系统线性定常系统，就是上一篇文章中的Φk,k−1\Phi_{k,k-1}Φk,k−1​是一个常值。为了更简单些，观测也用直接观测好了。于是这个定常系统可以这样表示：Xk=ΦXk−1+Wk−1X_k=\Phi X_{k-1}+W_{k-1}Xk​=ΦXk−1​+Wk−1​Zk=Xk+VkZ_k=X_k+V_kZk​=Xk​+Vk​这里的Φ\PhiΦ是常数了，观测Z和状态X都是标量，系统噪声WkW_{k}Wk​和量测噪声VkV_kVk​都是零均值的白噪声，方差矩阵

一、卡尔曼滤波的基本方程       经过前面三篇文章的铺垫，我们可以开始说说卡尔曼滤波器了。首先要说的是，卡尔曼滤波器的本质是线性最小方差估计。所以它也是最优估计的一种。可以认为卡尔曼滤波是线性最小方差估计的递推形式。还记得最小二乘估计吧，那个也有递推形式就是RLS算法。Pk+1=Pk−PkHk+1T(Wk+1−1+Hk+1PkHk+1T)−1Hk+1PkP_{k+1}=P_k-P_kH^T_{k+1}(W_{k+1}^{-1}+H

文章目录一、最小方差估计例子二、线性最小方差估计三、其他最优估计1、极大验后估计2、极大似然估计四、总结一、最小方差估计       前两篇文章对最优估计中的最小二乘估计进行了较为系统的介绍。我们已经知道，所谓最优估计就是让估值结果的某一项最优指标达到最小，而最小二乘估计的指标是让量测的方差最小。之前也说了，正因为是让这个指标是让量测最优，所以并不能保证估计最优。      

文章目录五、加权的最小二乘法六、递推最小二乘估计（RLS）又一个例子七、最小二乘法总结五、加权的最小二乘法       上一篇文章介绍了最小二乘估计的基本原理和估计精度，并且举了例子来说明这个最小二乘估计是如何应用的。在上一篇最后，我提了一个概念，就是最小二乘估计精度不高。为什么这么说呢？还是用一个例子来说明吧。       跟上一个例子类似，也是通过一台仪器对一