卡尔曼滤波器的实用方法及其实现方法

前言

卡尔曼滤波器对于不熟悉的人来说就是一种算法,它使用随时间观察的一系列观量值,,加速度计和陀螺仪在测量值是就会包含测量误差的噪声.卡尔曼滤波器将尝试根据当前和以前的状态来估计系统的状态,这往往比测量更加的精准.问题在于机器人来回的移动,加速度计在用于测量重力加速度时通常噪声会很大.陀螺仪则会随着时间的推移而漂移.

工作原理

卡尔曼滤波器的工作原理根据测量值生成系统统计的最优估计值.为此,它需要知道滤波器输入的噪声(测量噪声)以及系统本身的噪声(过程噪声).为此,噪声必须是高斯分布的,并且平均值是0,巧的是大部分都是这样.

系统状态 $X_{k}$

当矩阵是常数并且不依赖于当前时间时,你不必在它们后面写k.因此,例 $F_k$ 可以简化为F 。
我对符号的进行一个简短解释:
1,首先,我记录所谓的previous state(之前状态):
$\boldsymbol{\hat{x}}_{k-1 | k-1}$
这是先前状态和之前状态的估计值得出的先前估计状态.

2,先验状态:
$\boldsymbol{\hat{x}}_{k | k-1}$
先验是指基于系统前一状态和之前状态的估计值,对当前时间K的状态矩阵的估计值.

3,后验状态:
$\boldsymbol{\hat{x}}_{k | k}$
后验是指基于定截至时间并包括k的观测值在时间K的状态的估计值.

问题是系统状态本身是隐藏的,只能通过观察 $z_k$ 来观察.这也称作为隐马尔可夫模型(HMM).(隐马尔可夫模型:简单来说就是Y这个结果值你能观察到,但是里面的观测值你无法直接观察.HMM有一个额外的要求就是Y在 $t=t_{0}$ 必须完全受X在 $t=t_{0}$ ,以及X和Y在 $t<t_{0}$ 时必须完全独立于Y在 $t=t_{0}$ 鉴于X在 $t=t_{0}$ .可以使用最大似然法,估计HMM中的参数,对于线性链HMM,可以使用Baum-Welch算法(它是一个期望最大化算法的特例)

Baum-Welch算法的示例:

假设我们有一只鸡，我们每天中午都会从中收集鸡蛋。现在鸡是否下蛋收集取决于一些隐藏的未知因素。然而，我们可以（为简单起见）假设鸡总是处于影响鸡是否下蛋的两种状态之一，并且这种状态仅取决于前一天的状态。现在我们不知道初始起点的状态，我们不知道两种状态之间的转换概率，我们不知道鸡在给定特定状态下蛋的概率。首先，我们首先猜测过渡矩阵和排放(发射)矩阵。

然后，我们进行一组观察（E = 鸡蛋，N = 无鸡蛋）：N、N、N、N、N、E、E、N、N、N

这为我们提供了一组观察到的日期之间的转换：NN、NN、NN、NN、NE、EE、EN、NN、NN

下一步是估计新的转换矩阵。例如，序列 NN 的概率和状态为 $S_{1}$ ⁠⁠然后 $S_{2}$ ⁠由以下给出，P(S1)⋅P(N|S1)⋅P(S1→S2)⋅P(N|S2).

因此，对⁠S1自⁠S2现在是 ${\frac {0.22}{2.4234}}=0.0908$ (在下表中称为“伪概率”）。然后，我们计算⁠⁠自⁠S1,⁠S2⁠自⁠S和⁠S1自⁠S1transition probabilities 并标准化，以便它们加到 1。这为我们提供了更新的过渡矩阵：

接下来，我们要估计一个新的发射矩阵:

排出(发射)矩阵的新估计来自⁠S现在 ${\displaystyle {\frac {0.2394}{0.2730}}=0.8769}.$
这允许我们通过将各个观察到的序列的概率相加来计算上述算法中描述的发射矩阵。然后我们重复 N 是否来自⁠S如果 N 和 E 来自⁠S⁠并正常化。

为了估计初始概率，我们假设所有序列都以 hidden 状态开头⁠S1⁠并计算最高概率，然后重复⁠S2然后我们再次进行归一化以给出更新的初始向量。最后，我们重复这些步骤，直到结果的概率令人满意地收敛。这是解决一个典型的时间序列数据建模与推理判断问题.
1,收敛的含义就是:通过真实值迭代来逼近问题的最佳值.
2,HMM 的核心是概率的估计，而这些概率（初始概率、转移概率、发射概率）一开始都是未知的.

系统状态 $X_{k}$

这就意味着该状态将基于时间k的状态和所有之前的状态.这也意味着你不能相信卡尔曼滤波器之前的状态估计值.
$\hat{x}$ 这上面有个帽子的意思是:对这个状态的估计值.与单个x不同,单个x表示真实状态-我们试图估计的.
因此,时间k的状态符号为: $\boldsymbol{x}_k$
时间k的系统状态(如果由下示给出):
$\boldsymbol{x}_k = \boldsymbol{F}x_{k-1} + \boldsymbol{B}u_k + w_k$

其中 $\boldsymbol{x}_k$ 是状态矩阵,由下式给出:
$\boldsymbol{x}_k = \begin{bmatrix} \theta \\ \dot{\theta}_b \end{bmatrix}_k$
如您所见,滤波器的输出将是角度 $\theta$ ,但也是基于加速度计和陀螺仪测量值的偏差 $\dot{\theta _b{_{_{_{_{_{}}}}}}}$ .偏差是陀螺仪漂移的量.这一意味着可以通过从陀螺仪测量中减去偏移量来获得真实速率.
接下来是F矩阵,它是应用于prevouis状态 $x_{k-1}$ 的状态转化模型.
在这种情况下F,定义为:
$\boldsymbol{F} = \begin{bmatrix} 1 & -\Delta t \\ 0 & 1 \end{bmatrix}$

我知道这可能 $-\Delta t$ 看起来令人困惑,但稍后会有意义(作者评论打不开)
接下来是控制输入 $u_{k}$ ,它是陀螺仪在时间k测量单位为度/秒(°/s),这也称之为速率 $\dot{\theta }$ .我们实际上要将状态方程改写为:
$\boldsymbol{x}_k = \boldsymbol{F}x_{k-1} + \boldsymbol{B}{\dot{\theta}_k} + w_k$
接下来是B矩阵.则称为控制输入模型,其定义为:
$\boldsymbol{B} = \begin{bmatrix} \Delta t \\ 0 \end{bmatrix}$
这是完全有道理的,因为当你将速率 $\dot{\theta }$ 乘以时间差 $\Delta t$ 时,你将获得角度,并且由于我们无法直接根据速率计算偏差,所以我们设置为0.

$w_{k}$ 是过程噪声,它是属于高斯分布的,均值为零,与时间k有协方差Q:
$\boldsymbol{w}_k \sim N\left ( 0, \boldsymbol{Q}_k \right )$
$Q_k$ 是过程噪声协方差矩阵,在本例中为加速度计和偏置的状态估计值的协方差矩阵,这种情况下,我们将认为biac(偏置)和accelerometer加速度计的估值是独立(不受对方影响,所以相当于再求自身的协方差)的,因此它实际上刚好等于accelerometer和bias的估计值的方差.(方差其实是随机变量与自身的协方差)
最终矩阵定义如下: $\boldsymbol{Q}_k = \begin{bmatrix} Q_\theta & 0 \\ 0 & Q_{\dot{\theta}_b} \end{bmatrix}\Delta t$

如你所见, $Q_{k }$ 协方差矩阵取决于当前时间k,因此加速度计方差 $Q_{\theta }$ 和偏差 $Q_{\dot{\theta _b{}}}$ 方差乘以时间差 $\Delta t$ .
距离上次更新状态以来的时间越长,进程噪声就越大.比如陀螺仪已经漂移了.
你必须知道这些常数才能使卡尔曼滤波器工作.
请注意,如果设置的值较大,则状态估计中的噪声越多.因此,比如估计的角度开始漂移,则必须增加 $Q_{\dot{\theta _b{}}}$ 的值.(我们需要这个偏置方差矩阵构成时间k系统状态方程,所以这个估计角度开始漂移时,这个 $Q_{\dot{\theta _b{}}}$ 也得增加).否则如果估计值往往很慢,则表示你过于信任角度的估计值,并应该尝试增加 $Q_{\theta }$ 的值,以便快速响应(这是我想的原文是减少我觉得不对).

测量 $z_{k}$

真实状态 $x_{k}$ 的观察或测量 $z_{k}$ ,观察 $z_{k}$ 结果由下式给出:
   $\boldsymbol{z}_k = \boldsymbol{H}x_{k} + v_k$
测量值 $z_{k}$ 有当前状态 $x_{k}$ 乘以H矩阵加上测量噪声 $v_{k}$ 给出.
H称为观测模型(Observation Model),用于将真实状态空间映射到observed空间.无法观察到真实状态.由于测量只是来自加速度计的测量结果,H因此又下式给出:
                                                                                              $\boldsymbol{H} = \begin{bmatrix} 1 & 0 \end{bmatrix}$
测量的噪声也必须式高斯分布的,均值为0,R协方差为:
$v_{k}\sim N(0,R)$
高斯分布:若随机变量X服从一个平均数为μ、为的正态分布，则记为：X∼N(μ,σ2),则其g概率密度函数为 $f(x)={\frac {1}{\sigma {\sqrt {2\pi }}}}\;e^{-{\frac {\left(x-\mu \right)^{2}}{2\sigma ^{2}}}}\!$ .

但是,由于R不是矩阵,因为测量噪声刚好等于测量的方差,应为同一变量的协方差等于方差.
现在我们可以定义R如下: `
$\boldsymbol{R} = E \begin{bmatrix} v_k & {v_k}^T \end{bmatrix} = var(v_k)$
我们假设测量噪声方差设置var(v)得太高,滤波器得响应速度会非常慢,因为他对新数据的信任度比较低,但如果它太小了,该值就会产噪声,因为我们会过于相信加速度计的测量数据值.
所以要找到过程噪声方差 $Q_\theta$ 和 $Q_{\dot{\theta}_b}$ 测量噪声var(v)的测量方差.有很多种可以找到他们,本文不讨论他们.

卡尔曼滤波方程

预测

在前两个方程中,我们将尝试预测时间k的当前状态和误差协方差矩阵.首先,过滤器将尝试根据所有先前的状态和陀螺仪的测量来估计当前的状态:
$\boldsymbol{\hat{x}}_{k | k-1} = \boldsymbol{F}\hat{x}_{k-1 | k-1} + \boldsymbol{B}{\dot{\theta}_k}$

这也是为什么他被成为控制输入的原因,应为我们将其用做额外的输入来估计当前时间k的状态,称为先验状态 $\hat{x}_{k | k-1}$ ,如本文开头所述.
接下来的事情是,我们将尝试根据起前面的误差协方差矩阵来估计先验误差协方差矩阵 $P_{k | k-1}P_{k-1 | k-1}$ ,其定义如下:
$\boldsymbol{P}_{k | k-1} = \boldsymbol{F}\boldsymbol{P}_{k-1 | k-1}\boldsymbol{F}^T + \boldsymbol{Q}_k$

这里为什么要乘以F和F的转置,这就和HMM模型有关,他始终会趋近于一个固定的值.
此矩阵用于估计状态的当前值的信任程度.越小,我们就越信任当前的估计状态(因为这里是协方差本质和方差差不太多),上述方程的原理实际上很容易理解,因为很明显.乘以F和F的转置并在时间k处添加过程噪声 $Q_{k}$ .
在我们的例子中,误差矩阵P是一个2*2矩阵:
$\boldsymbol{P} = \begin{bmatrix} P_{00} & P_{01} \\ P_{10} & P_{11} \end{bmatrix}$

更新

我们要做的第一件事是计算测量 $z_k$ 和先验 $x_{k | k-1}$ 之间的差异,这也称之为创新:
$\boldsymbol{\tilde{y}}_k = \boldsymbol{z}_k - \boldsymbol{H}\hat{x}_{k | k-1}$

观测模型H用于将先验状态映射到观察空间,这是来自加速度计的测量,因此创新不是一个矩阵
   $\boldsymbol{\tilde{y}}_k = \begin{bmatrix} \boldsymbol{\tilde{y}} \end{bmatrix}_k$
接下来我们要做的就是计算所谓的创新协方差：
                                                                 $\boldsymbol{S}_k = \boldsymbol{H} \boldsymbol{P}_{k | k-1} \boldsymbol{H}^T + \boldsymbol{R}$
它的作用是,它试图根据先验误差协方差矩阵和测量协方差矩阵 $P_{k | k-1}R$ 来预测我们应该对测量的信任程度.观测模型H用于先验误差协方差矩阵 $P_{k | k-1}$ 映射到观测空间.
测量噪声的值越大,S就越大,这意味着我们不太信任输入的测量结果.
在本例中,S不是矩阵,只是写成:                 $\boldsymbol{S}_k = \begin{bmatrix} \boldsymbol{S} \end{bmatrix}_k$
下一步是计算卡尔曼增益.卡尔曼增xing益用于表示我们对创新的信任程度,定义为:
                                                                 $\boldsymbol{K}_k = \boldsymbol{P}_{k | k-1} \boldsymbol{H}^T \boldsymbol{S}_k^{-1}$
如果我们不那么信任创新,那么创新协方差S就会很高,如果我们信任状态的估计,南无误差协方差举证P就会很小,因此,如果我们信任创新但是不信任当前状态的估计,卡尔曼滤波就会很小.
如果深入地观察,你会发现观测模型H转置用于将状态误差协方差矩阵P的状态映射到观测空间.然后,我们通过于创新协方差矩阵的倒数相乘来比较误差协方差矩阵S.
这是有道理的,因为我们将使用观测模型H从状态误差协方差中提取数据,并将其于当前创新协方差的估计进行比较.
请注意,如果你不知道启动时的状态,这可以像这样设置误差协方差矩阵:
                                                                         $\boldsymbol{P} = \begin{bmatrix} L & 0 \\ 0 & L \end{bmatrix}$
其中的L表示一个很大的数字.
对于我的平衡机器人,我知道其实角度,并通过校准找到陀螺仪在启动时的偏差,因此我假设在启动时状态是已知的,因此我想这样初始化误差协方差矩阵:
$\boldsymbol{P} = \begin{bmatrix} 0 & 0 \\ 0 & 0 \end{bmatrix}$
在这种情况下,卡尔曼增益时一个2*1矩阵:

$\boldsymbol{K} = \begin{bmatrix} K_0 \\ K_1 \end{bmatrix}$
现在我们可以更新当前状态的后验估计:
                                                                       $\boldsymbol{\hat{x}}_{k | k} = \boldsymbol{\hat{x}}_{k | k-1} + \boldsymbol{K}_k \; \boldsymbol{\tilde{y}}_k$
这是通过卡尔曼增益乘以创新的先验状态相加来完成的.
请记住,创新时测量和估计的先验状态之间的插值,因此创新既可以是积极的,也可以是消极的.
稍微简化一下方程可以理解为我们简单地纠正先验状态的估计值,估计是使用前一个状态和陀螺仪测量计算的,测量-在本例中为加速度计.
最后一件事就是更新后验误差矩阵:            $\boldsymbol{P}_{k | k} = (\boldsymbol{I} - \boldsymbol{K}_k \boldsymbol{H}) \boldsymbol{P}_{k | k-1}$
其中I称为单位矩阵,定义为:                                   $\boldsymbol{I} = \begin{bmatrix} 1 & 0 \\ 0 & 1 \end{bmatrix}$

如果你能仔细的看到这里你肯定是一个很厉害的人,以下我对它的总结你也可以谢谢你自己的总结.(对学习很有用)

心得体会:我觉得肯定里面的有些数学公式各位看不懂但是不重要,你要整体的思路是什么,你单看公式肯定是看不懂的因为写他的人是通过思路和计算写出来的,不要觉得这个很难.要有像这种能提升自己的知识亮剑.
卡尔曼滤波通常是和PID一起使用,kalman主要的作用就是减少传感器的数据和真实数据之间的误差,这样可以使物体运动起来更丝滑,不负责控制运动,不要被有些视频误导.
还有数学公式的推导是从发现这个模型的现象然后进行客观的分析现象的原因,然后再次重现现象数据进行数学推导,得出最美的简式,像最后的更新后验误差矩阵就是如此.如果你从等式开始想你就会陷入死胡同,但是你从整个公式的推导你就明白了,就是方差的推导,然后将卡尔曼增益带进去,有个注意点是卡尔曼增益怎么求来的,就是方差最小的时候求出的增益.

总体来说,卡尔曼滤波就是这样的,比如现在有一个小车,你知道小车的公式吧,我只需要知道速度的就能知道位移但是会有噪声干扰小车这个噪声是属于系统的噪声系统噪声（过程噪声）：小车的运动受限于模型不完美、外界干扰（比如摩擦力、风阻等），这些无法完全消除的随机影响会导致系统状态（如位移、速度）有误差。这种噪声是正态分布的。
,当到达位置时实际的位移和你通过公式算的位移肯定不一样,因为有系统误差,但是当数据多的时候它也会处于一个固定的值的区间震荡,区间很小.假设小车上也有一个位移计,能判断你的位置,测量噪声：位移传感器（或者速度传感器）的测量值也会受到外界环境或传感器自身精度的影响，从而产生误差，这种误差也被假设为正态分布。
卡尔曼滤波在这一部分做的事情就是利用 系统模型 对当前位置进行预测，但由于噪声的存在，这个预测值不是完美的，会有误差。

此时，卡尔曼滤波器会将 预测值 和 测量值 结合起来，通过 权重分配 计算出一个更接近真实状态的 估计值。

然后用协方差矩阵表示位移计和实际位移的偏差方差,当这个数值最小时,就是最有可能发生的情况,就会求出kalman增量,记得一定要更新后验误差矩阵因为它是在多个数据后才会趋于稳定,一定要更新修正协方差矩阵.(更新后的误差协方差矩阵变小，表示随着时间的推移，系统的估计值越来越接近真实值。不断地迭代更新后，系统状态估计会趋于稳定，估计的误差范围会收敛到一个较小的区间。)
总结

卡尔曼滤波通过预测和测量两步来估计系统的状态。
由于噪声（系统噪声与测量噪声）的存在，预测和测量都有误差。
卡尔曼滤波通过 协方差矩阵 衡量不确定性，计算 卡尔曼增益，使估计值在“预测值”和“测量值”之间进行平衡。
更新 后验误差协方差矩阵，随着时间的迭代，估计值会更加接近真实值，误差范围收敛到一个小的区间。

预测位移：通过速度和时间步长计算。
测量位移：通过传感器提供的测量值。
校正位移：结合预测和测量，得出最优估计值。

最终，小车的位移估计将更加平滑，误差也会减小。这是卡尔曼滤波的核心优势！

以下是matlab代码,有兴趣的同学可以去玩玩看.

classdef kaerman
    % Kalman滤波器实现，用于角度估计
    properties
        Q_angle = 0.001;  % 角度的过程噪声协方差
        Q_bias = 0.003;   % 偏置的过程噪声协方差
        R_measure = 0.03; % 测量噪声协方差
        
        angle = 0;        % 估计的角度
        bias = 0;         % 偏置估计
        rate = 0;         % 无偏角速度

        P = [0 0; 0 0];   % 误差协方差矩阵
    end
    
    methods
        % 构造函数：初始化滤波器参数
        function obj = kaerman(Q_angle, Q_bias, R_measure)
            if nargin > 0
                obj.Q_angle = Q_angle;
                obj.Q_bias = Q_bias;
                obj.R_measure = R_measure;
            end
        end
        
        % 卡尔曼滤波器的主函数
        % 输入：newAngle - 测量到的角度
        %       newRate - 测量到的角速度
        %       dt - 时间步长
        function [obj, angle_out] = getAngle(obj, newAngle, newRate, dt)
            % **预测阶段（时间更新）**
            % 步骤1：状态预测
            obj.rate = newRate - obj.bias; % 去除偏置的角速度
            obj.angle = obj.angle + dt * obj.rate; % 预测角度（x = x + dt * rate）

            % 步骤2：误差协方差矩阵的预测
            obj.P(1,1) = obj.P(1,1) + dt * (dt * obj.P(2,2) - obj.P(1,2) - obj.P(2,1) + obj.Q_angle);
            obj.P(1,2) = obj.P(1,2) - dt * obj.P(2,2);
            obj.P(2,1) = obj.P(2,1) - dt * obj.P(2,2);
            obj.P(2,2) = obj.P(2,2) + obj.Q_bias * dt;

            % **更新阶段（测量更新）**
            % 步骤3：计算卡尔曼增益
            S = obj.P(1,1) + obj.R_measure; % 预测误差协方差 + 测量噪声协方差
            K = [obj.P(1,1) / S; obj.P(2,1) / S]; % 卡尔曼增益（2x1向量）

            % 步骤4：更新状态估计
            y = newAngle - obj.angle; % 测量残差（测量值与预测值之差）
            obj.angle = obj.angle + K(1) * y; % 更新角度
            obj.bias = obj.bias + K(2) * y; % 更新偏置

            % 步骤5：更新误差协方差矩阵
            P_temp = obj.P; % 暂存P矩阵
            obj.P(1,1) = obj.P(1,1) - K(1) * P_temp(1,1);
            obj.P(1,2) = obj.P(1,2) - K(1) * P_temp(1,2);
            obj.P(2,1) = obj.P(2,1) - K(2) * P_temp(1,1);
            obj.P(2,2) = obj.P(2,2) - K(2) * P_temp(1,2);

            % 返回估计的角度
            angle_out = obj.angle;
        end
        
        % 设置初始角度
        function obj = setAngle(obj, angle)
            obj.angle = angle;
        end
        
        % 获取当前无偏角速度
        function rate = getRate(obj)
            rate = obj.rate;
        end
        
        % 调整滤波器参数
        function obj = setQangle(obj, Q_angle)
            obj.Q_angle = Q_angle;
        end
        
        function obj = setQbias(obj, Q_bias)
            obj.Q_bias = Q_bias;
        end
        
        function obj = setRmeasure(obj, R_measure)
            obj.R_measure = R_measure;
        end
        
        % 获取滤波器参数
        function Q_angle = getQangle(obj)
            Q_angle = obj.Q_angle;
        end
        
        function Q_bias = getQbias(obj)
            Q_bias = obj.Q_bias;
        end
        
        function R_measure = getRmeasure(obj)
            R_measure = obj.R_measure;
        end
    end
end

以上的代码是公式化了简的形式.

% 创建 Kalman 滤波器对象
kalmanFilter = kaerman(); % 确保 Kalman 类文件在同一目录

% 设置初始角度
kalmanFilter = kalmanFilter.setAngle(0);

% 初始化时间和角度存储数组
dt = 0.01; % 时间步长（单位：秒）
totalTime = 4; % 总仿真时间（单位：秒）
time = 0:dt:totalTime; % 时间数组
estimatedAngles = zeros(size(time)); % 估计角度数组

% 动态输入数据：模拟角速度和测量角度
trueAngle = 0; % 初始真实角度
trueAngles = zeros(size(time)); % 存储真实角度
measuredAngles = zeros(size(time)); % 存储测量角度
angularRates = zeros(size(time)); % 存储角速度

for i = 1:length(time)
    % 动态角速度 - 使用正弦波模拟角速度的变化
    newRate = 5 * sin(0.5 * time(i)); % 角速度在 [-5, 5] 范围内变化
    angularRates(i) = newRate; % 存储角速度

    % 真实角度 - 通过积分角速度得到
    trueAngle = trueAngle + newRate * dt;
    trueAngles(i) = trueAngle; % 存储真实角度

    % 测量角度 - 加上随机噪声来模拟测量误差
    noise = randn * 0.5; % 随机噪声，标准差为 0.5 度
    newAngle = trueAngle + noise; % 测量角度
    measuredAngles(i) = newAngle;

    % 运行卡尔曼滤波器
    [kalmanFilter, estimatedAngle] = kalmanFilter.getAngle(newAngle, newRate, dt);
    estimatedAngles(i) = estimatedAngle; % 存储估计角度
end

% 绘制结果
figure;

% 1. 绘制真实角度、测量角度和卡尔曼滤波结果
plot(time, trueAngles, 'g-', 'LineWidth', 2); hold on; % 真实角度 (绿色)
plot(time, measuredAngles, 'r.', 'MarkerSize', 8); % 测量角度 (红色点)
plot(time, estimatedAngles, 'b-', 'LineWidth', 2); % 卡尔曼滤波估计角度 (蓝色)

grid on;
xlabel('时间 (秒)');
ylabel('角度 (度)');
title('卡尔曼滤波器估计角度与真实角度对比');
legend('真实角度', '测量角度', '估计角度');

实验结果: