第七讲状态估计-卡尔曼滤波

卡尔曼滤波原理
卡尔曼滤波是一种用于估计系统状态的递归滤波器。

它可以通过组合系统的测量值和模型的预测值来提供对状态的最优估计。

卡尔曼滤波器首先利用系统的数学模型预测下一个状态，并计算预测值与实际测量值之间的差异。

然后，通过加权这些差异，卡尔曼滤波器可以生成对当前状态的最佳估计。

卡尔曼滤波的核心原理是“最小均方误差”。

它假设系统状态和观测都是高斯分布，然后尝试寻找最小均方误差的估计值。

通过选择合适的权重，卡尔曼滤波器可以在预测值和测量值之间找到一个平衡，从而提供最佳的估计结果。

卡尔曼滤波器由两个主要步骤组成：预测和更新。

在预测步骤中，卡尔曼滤波器使用系统模型和先前的状态估计来预测下一个状态。

然后，在更新步骤中，卡尔曼滤波器将测量值与预测值进行比较，并使用加权平均法来更新状态估计。

通过周期性地重复这两个步骤，卡尔曼滤波器可以连续地提供对系统状态的估计。

卡尔曼滤波器在估计问题中广泛应用，特别是在传感器融合、航空航天和导航系统中。

它能够有效地处理噪声和不确定性，并在给定系统模型和测量信息的情况下提供最优的状态估计。

附录：kalman滤波（起源、发展、原理、应用）1、Kalman滤波起源及发展1960年，匈牙利数学家卡尔曼发表了一篇关于离散数据线性滤波递推算法的论文，这意味着卡尔曼滤波的诞生。

斯坦利.施密特(Stanley Schmidt)首次实现了卡尔曼滤波器，卡尔曼在NASA埃姆斯研究中心访问时，发现他的方法对于解决阿波罗计划的轨道预测很有用，后来阿波罗飞船的导航电脑使用了这种滤波器。

关于这种滤波器的论文由Swerling (1958)、Kalman (1960)与Kalman and Bucy (1961)发表.卡尔曼滤波是一种有着相当广泛应用的滤波方法，但它既需要假定系统是线性的，又需要认为系统中的各个噪声与状态变量均呈高斯分布，而这两条并不总是确切的假设限制了卡尔曼滤波器在现实生活中的应用。

扩展卡尔曼滤波器（EKF）极大地拓宽了卡尔曼滤波的适用范围。

EKF的基本思路是，假定卡尔曼滤滤对当前系统状态估计值非常接近于其真实值，于是将非线性函数在当前状态估计值处进行台劳展开并实现线性化。

另一种非线性卡尔曼滤波叫线性化卡尔曼滤波。

它与EKF的主要区别是前者将非线函数在滤波器对当前系统状态的最优估计值处线性化，而后者因为预先知道非线性系统的实际运行状态大致按照所要求、希望的轨迹变化，所以这些非线性化函数在实际状态处的值可以表达为在希望的轨迹处的台劳展开式，从而完成线性化。

不敏卡尔曼滤波器（UKF）是针对非线性系统的一种改进型卡尔曼滤波器。

UKF处理非线性系统的基本思路在于不敏变换，而不敏变换从根本上讲是一种描述高斯随机变量在非线性化变换后的概率分布情况的方法。

不敏卡尔曼滤波认为，与其将一个非线性化变换线性化、近似化，还不如将高斯随机变量经非线性变换后的概率分布情况用高斯分布来近似那样简单，因而不敏卡尔曼滤波算法没有非线性化这一步骤。

在每一定位历元，不敏卡尔曼滤波器按照一套公式产生一系列样点，每一样点均配有一个相应的权重，而这些带权的样点被用来完整地描述系统状态向量估计值的分布情况，它们替代了原先卡尔曼滤波器中的状态向量估计值及协方差。

卡尔曼滤波法卡尔曼滤波法是指一种基于相关理论的有状态估计技术，它有效地融合已知信息和新观测信息，以确定系统内部状态如位置、速度和加速度等。

一、特点1、适用于高维系统：卡尔曼滤波能够在高维系统状态空间中进行有效滤波。

2、易于实现：卡尔曼滤波一般采用分块矩阵，以便在离散时间序列上进行数值计算，从而使估计工作更加便捷。

3、具有自适应性：系统变化或者观测值的不确定性时，卡尔曼滤波算法能够自动调整估计系数，从而实现准确的估计。

二、原理卡尔曼滤波尝试将状态空间中的信息和观测数据结合，来估计系统的状态变量，它有两个基本阶段:状态预估和状态校正。

首先，采用现有的状态信息和系统模型，利用预测模型来预测可能的状态取值。

然后，通过观测数据，不断更新这些状态估计值，使估计值更加准确。

三、应用1、航空航天：卡尔曼滤波算法被广泛应用于气球导航中，可以对空中对象的位置、速度、加速度等敏感参数进行更精准的测量。

2、自动控制：卡尔曼滤波算法用于控制系统建模，可以用于制定自动跟踪控制程序，实现实时系统参数调节。

3、惯性导航：卡尔曼滤波算法用于飞行器间机动状态的跟踪，对于导弹、衛星等航空器定位和姿态测量提供有效的解决方案。

4、机器人：卡尔曼滤波可以应用于机器人的自运动控制，可以帮助机器人在复杂环境中精准定位。

四、优缺点优点：1、结构灵活：卡尔曼滤波因为结构灵活的缘故，可以容易地应用于各种复杂的有状态系统。

2、收敛速度快：卡尔曼滤波收敛速度快、具有自适应性，可以进行准确的状态估计。

缺点：1、需要假定系统模型：卡尔曼滤波可以实现有效的数据融合，但需要满足一定的系统假定。

2、状态估计的不稳定性：卡尔曼滤波的状态估计结果受观测信息的影响很大，估计值可能受到观测噪声的很大影响。

卡尔曼滤波详解卡尔曼滤波(Kalman Filter)是一种用于估计系统状态的算法，广泛应用于控制系统、信号处理、机器人导航等领域。

卡尔曼滤波的核心思想是利用系统的动态模型和测量数据的信息来对系统状态进行估计，同时最小化估计误差的方差。

在实际应用中，卡尔曼滤波能够对系统状态进行高效、准确的估计，并具有良好的鲁棒性和适应性。

卡尔曼滤波的核心思想可以简单概括为“测量并补偿”，即先通过传感器测量得到当前的状态信息，然后利用系统动态模型来预测下一时刻的状态，再将测量值与预测值进行比较，通过加权平均的方式得到最终的估计值。

要实现这个过程，需要建立卡尔曼滤波的基本模型，包括状态转移方程、观测方程、协方差矩阵和初始状态。

卡尔曼滤波的核心步骤包括预测阶段和更新阶段。

预测阶段主要利用系统动态模型对状态进行预测，以及计算预测误差的方差。

预测阶段包括以下几个步骤：1. 状态预测：根据系统动态模型和当前状态估计值，预测下一时刻的状态估计值。

2. 协方差预测：根据系统动态模型和当前状态协方差矩阵，预测下一时刻的协方差矩阵。

3. 估计误差的量化：计算预测值与真实值之间的估计误差，以及预测误差的方差。

更新阶段主要利用测量数据对状态进行修正，以及更新协方差矩阵。

更新阶段包括以下几个步骤：1. 估计增益：根据协方差矩阵和观测噪声方差，计算估计值与观测值之间的加权比例。

2. 状态修正：利用估计增益和测量值对状态进行修正。

3. 协方差修正：利用估计增益对协方差矩阵进行修正。

卡尔曼滤波的应用非常广泛，包括导航系统、车辆控制、信号处理、自动驾驶、机器人导航等领域。

卡尔曼滤波能够对系统状态进行高效、准确的估计，并且具有良好的鲁棒性和适应性，对噪声和误差具有较好的鲁棒性。

此外，卡尔曼滤波具有良好的数学基础和理论支撑，能够直接应用于许多复杂的系统中。

卡尔曼滤波状态估计步骤哎呀，这卡尔曼滤波啊，真是个让人头疼的玩意儿。

不过，别急，咱们慢慢聊聊，就像咱们在咖啡馆里闲聊一样。

首先，得说，卡尔曼滤波这玩意儿，就像是你有个朋友，他总是给你发位置信息，但是呢，这信息有时候准，有时候不准。

你呢，就得用这个卡尔曼滤波来猜他到底在哪儿。

听起来是不是挺有意思的？咱们先从第一步说起，就是初始化。

这就像是你刚认识这个朋友，你对他的了解还不多，只能根据他给你的第一个信息，也就是位置，来猜测他在哪儿。

这时候，你心里可能有点没底，因为你不知道这个信息有多靠谱，所以你会给他一个“不确定性”的标签，比如，你可能会想：“这家伙是不是在跟我开玩笑呢？”接下来，就是预测步骤了。

这就像是你根据你朋友以前给你的信息，来预测他下一步会去哪儿。

比如，你知道他每天下班都会去公园散步，所以今天你也会猜他可能还会去那儿。

但是，你心里还是有点不确定，因为人嘛，总有变数，对吧？然后，就是更新步骤了。

这一步最有意思，就像是你朋友突然给你发了一条信息，告诉你他现在在公园的长椅上坐着。

这时候，你得重新评估他的位置，因为你有了新的信息。

你得把之前的猜测和新的消息结合起来，就像是你在地图上画了两条线，然后找到它们的交点，这个交点，就是你朋友现在最可能的位置。

最后，就是评估不确定性。

这就像是你在更新位置后，还得想想：“这次的消息靠谱吗？我是不是还得留点余地？”所以，你会给你的新位置再打个“不确定性”的标签，这个标签会根据新旧信息的靠谱程度来调整。

你看，卡尔曼滤波就是这么个过程，就像是你在和朋友玩捉迷藏，你得根据他给你的线索，一步步猜他在哪儿。

虽然有时候可能会猜错，但是通过不断地更新和调整，你会越来越接近他的真实位置。

所以，卡尔曼滤波，其实就是个不断猜测和修正的过程，就像我们在生活中不断调整自己的判断一样。

虽然听起来有点复杂，但其实就跟我们平时聊天、猜测朋友心思差不多，挺有意思的，对吧？好了，聊了这么多，你是不是对卡尔曼滤波有点儿感觉了呢？其实，它并不像听起来那么高深莫测，它就像是我们日常生活中的一个小插曲，充满了不确定性和惊喜。

一、背景---卡尔曼滤波的意义随着传感技术、机器人、自动驾驶以及航空航天等技术的不断发展，对控制系统的精度及稳定性的要求也越来越高。

卡尔曼滤波作为一种状态最优估计的方法，其应用也越来越普遍，如在无人机、机器人等领域均得到了广泛应用。

对于Kalman Filter的理解，用过的都知道“黄金五条”公式，且通过“预测”与“更新”两个过程来对系统的状态进行最优估计，但完整的推导过程却不一定能写出来，希望通过此文能对卡尔曼滤波的原理及状态估计算法有更一步的理解。

二、卡尔曼滤波的基本模型假设一离散线性动态系统的模型如下所示：x_{k} = A*x_{k-1} + B*u_{k} + w_{k-1}-------(1)z_{k} = H*x_{k} + v_{k} --------------------(2)其中，各变量表征的意义为：———————————————————————————x_{k}\Rightarrow 系统状态矩阵，-------， z_{k}\Rightarrow 状态阵的观测量（实测）A\Rightarrow 状态转移矩阵，-------， B\Rightarrow 控制输入矩阵H\Rightarrow 状态观测矩阵w_{k-1}\Rightarrow 过程噪声，-------，v_{k}\Rightarrow 测量噪声———————————————————————————如果大家学过《现代控制理论》的话，对上述模型的描述形式一定不会陌生，只是多了变量 w_{k-1} 与 v_{k} 。

其中，随机变量w_{k-1} 代表过程噪声（process noise）， v_{k} 代表测量噪声（measurement noise），且为高斯白噪声，协方差分别为 Q 和 R ，即 p(w) \in N(0,Q) ， p(v) \in N(0,R) 。

为什么要引入这两个变量呢？对于大多数实际的控制系统（如倒立摆系统）而言，它并不是一个严格的线性时变系统(Linear Time System)，亦或系统结构参数的不确定性，导致估计的状态值x_{k} 存在偏差，而这个偏差值由过程噪声 w_{k} 来表征。