卡尔曼滤波原理和使用

#include <iostream>
#include <vector>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

class KalmanFilter {
public:
    KalmanFilter(const MatrixXd& A, const MatrixXd& B, const MatrixXd& C, const MatrixXd& Q, const MatrixXd& R) {
        this->A = A;
        this->B = B;
        this->C = C;
        this->Q = Q;
        this->R = R;

        x = MatrixXd::Zero(A.rows(), 1);
        P = MatrixXd::Identity(A.rows(), A.rows());
    }

    void predict() {
        x = A * x;
        P = A * P * A.transpose() + Q;
    }

    void update(const MatrixXd& z) {  //根据观测值z来更新估计状态X和状态协方差矩阵P
        MatrixXd y = z - C * x;
        MatrixXd S = C * P * C.transpose() + R;
        MatrixXd K = P * C.transpose() * S.inverse();

        x = x + K * y;
        P = (MatrixXd::Identity(A.rows(), A.rows()) - K * C) * P;
    }

    MatrixXd getState() const {
        return x;
    }

private:
    MatrixXd A;  // 状态转移矩阵
    MatrixXd B;  // 控制输入矩阵
    MatrixXd C;  // 观测矩阵
    MatrixXd Q;  // 过程噪声协方差矩阵
    MatrixXd R;  // 观测噪声协方差矩阵

    MatrixXd x;  // 状态向量
    MatrixXd P;  // 状态协方差矩阵
};

int main() {
    // 定义系统矩阵和噪声协方差矩阵
    MatrixXd A(2, 2);
    A << 1, 1,
         0, 1;

    MatrixXd B(2, 1);
    B << 0.5,
         1;

    MatrixXd C(1, 2);
    C << 1, 0;

    MatrixXd Q(2, 2);
    Q << 0.1, 0,
         0, 0.1;

    MatrixXd R(1, 1);
    R << 1;

    // 初始化滤波器
    KalmanFilter filter(A, B, C, Q, R);

    // 模拟观测数据
    vector<double> measurements = {1, 2, 3, 4, 5};

    // 执行滤波
    for (double measurement : measurements) {
        // 预测步骤
        filter.predict();

        // 更新步骤
        MatrixXd z(1, 1);
        z << measurement;
        filter.update(z);

        // 输出估计值
        MatrixXd state = filter.getState();
        cout << "Estimated state: " << state << endl;
    }

    return 0;
}

随着传感技术，机器人，自动驾驶等不断发展，对控制系统的精度以及稳定性要求越来越高。
卡尔曼滤波作为一种状态最优估计方法，应用也越来越普遍.
对于Kalman Filter的理解，用过都知道“黄金五条”公式，且通过“预测”和“更新”两个过程对系统进行最优估计。

简单来说，卡尔曼滤波就是将观测值（指从系统中获得的实际测量值，如：传感器测量值，GPS定位值等）
和控制量（系统中的外部输入，如：控制器输出的控制指令，系统的的运动状态等）
进行融合。得到对系统状态（描述某一时刻的状态量或状态向量，它由系统内部的各种物理量，状态变量组成。
如：对于飞行器来说，系统状态可以包括飞行器的位置，速度，姿态，加速度等信息）的估计值。来进行预测。

卡尔曼滤波：用来估计系统状态的算法，它通过测量结果和预测结果进行加权平均，来得到更准确的状态估计值。
包括两个方面：状态预测和状态更新。
状态预测：第一步预测系统的状态。这个预测是基于系统的上一个状态和控制输入（如果有的话)计算出来的。
预测结果包含了系统的状态和状态估计的不确定性（协方差矩阵）。
状态更新：第二步是将测量结果与预测结果进行加权平均，来得到更准确的状态估计值。加权平均的权重是由
预测结果的不确定性和测量结果的不确定性共同决定的。如果预测结果的不确定性很小，那么
预测结果占主导地位；如果测量结果的不确定性很小，那么测量结果会占主导地位。

提出背景：
火箭在发生过程中需要时刻预测自己的状态（位置，速度），我们可以用一些传感器获得这两个量，但由于
存在电离层和大气层的干扰，传感器的测量值有时误差会很大，我们需要一种方法，预测一个新的更加准确的，
且不完全依赖传感器返回值的状态信息。事实上，如果传感器的返回值非常离谱，那我们完全有理由不相信
此时的数据，因为火箭需要符合某个运动方程，即使存在一些干扰，火箭的状态也应该是在某个合理的范围内。
思路：把传感器观测值和理论预测值做加权平均。既不完全相信传感器，也不完全相信理论模型。

假设我们只需要观测一个值，即设计一个一维Kalman Filter。
由于理论模型和观测都存在相应干扰，假设二值均服从正态分布（生活中几乎到处都是正态分布）

理论模型值的概率分布和观测值的概率分布只能通过大量数据采样得到，这要求我们在应用上述方法
进行滤波前，应该通过大量观察对理论模型和观测模型的分布有一个准确的了解。实际工程我们一般
假设传感器具有某个确定分布的，这个分布可以对传感器进行一段时间的观测得到
（如 slam中经常会让系统静止两小时以计算传感器的分布）。而系统方差是会随着我们每一次进行
卡尔曼融合而改变的（因为状态矩阵a的影响），所以我们需要持续的计算系统方差---迭代。

我们能做的就是系统运行开始后计算每一次的理论值方差，根据这个方差和传感器方差
计算出融合权重，每次都要重新计算--迭代。

每进行一次观测，就需要对两个分布的方差重新计算（一般传感器的方差不重新计算，设为定值），
迭代下去。因为是迭代的，考虑第k个时刻，当前的最优估计值应该由根据之前时刻状态由运动模型
计算出来的理论值，和tk时刻的传感器测量值，融合计算出tk时刻的卡尔曼预测值。

注意：状态更新每次都需要上一时刻的真实状态作为输入，而我们无法获得真实的输入，只能拿上
一时刻有卡尔曼理论预测出来的作为输入计算理论值。

状态转移矩阵A（系统模型矩阵）：表示系统在一个离散时间步长内从一个状态转移到下一个状态时
的线性关系，是一个方阵。描述了系统的动态特性，它将当前时刻的状态向量与下一个时刻的状态
向量之间的关系进行描述。在卡尔曼滤波中，状态转移矩阵是通过系统的物理模型或者观测到的
数据来估计或确定的。它可以包括系统的动态方程，控制输入，噪声等因素。

控制输入矩阵B：是在系统状态转移过程中考虑外部控制输入对系统状态变化的影响而
引入放入一个矩阵，表示了系统状态变化对外部控制输入的响应关系。作用：增强卡尔曼滤波算法
对外部控制的建模能力，提高状态估计的准确性。通过调整控制输入矩阵，可以对系统的控制策略
进行优化，以达到更好的状态估计效果。

观察矩阵C：通常用于描述一个系统的观察模型。在某些应用中，我们可能只能观测到系统的一部分
状态或输出，而无法直接观测到完整的系统状态。观测矩阵则用来将系统的状态与观测之间建立映射关系。
具体说：对于一个系统，通常有一个状态向量x,表示系统的完整状态。而观测矩阵C则定义了如何从系统的
状态向量中选择出对应的观测量。
例：一个简单的线性系统，其中状态向量x包含了位置和速度两个状态变量。但我们只能通过传感器观测到
位置，无法直接观测到速度。这种情况下，观测矩阵C可以被定义为[1,0]，表示我们只能观测到状态向量
x的第一个元素，即位置。
观测矩阵在很多应用中都起到了重要的作用，例如：卡尔曼滤波，系统辨识，控制系统设计等领域。
通过观测矩阵，我们可以将系统的状态与观测之间建立联系，从而实现对系统的检测，控制或估计。

协方差矩阵：用于描述两个或多个随机变量之间关系的矩阵。它反映了不同变量之间的相关性以及它们的方差。
假设我们有n个随机变量，它们可以表示为一个n维向量X=[X1,X2,...,Xn]。协方差矩阵记作Σ，是个nxn的矩阵，
其中的元素表示不同变量之间的协方差。
协方差表示了两个随机变量之间的线性关系。如果两个变量的协方差为正数，表示它们之间有正相关关系，
即当一个变量增加时，另一个变量也会增加。如果协方差为负数，表示它们之间存在负相关关系，即当一个
变量增加时，另一个变量会减少。当接近0时，表示两个变量之间没有线性相关性。
协方差矩阵Σ的对角线元素表示各个变量的方差，即Var(Xi)，其中i表示第i个变量。对角线之外的元素表示
不同变量之间的协方差，即Cov(Xi, Xj)，其中i和j表示不同的变量。
协方差矩阵具有以下性质：
1、对角线元素非负，即Var(Xi) ≥ 0；
2、对称性，即Cov(Xi, Xj) = Cov(Xj, Xi)；
3、协方差矩阵是半正定的，即对于任意非零向量a，有a^TΣa ≥ 0，其中a^T表示向量a的转置。
例：假设有两个随机变量X和Y，它们的取值如下表所示：
X(1,2,3,4,5),Y(3,5,7,8,11)
我们可以计算X和Y的协方差矩阵。首先，计算出X和Y的均值：
mean(X)=(1+2+3+4+5)/5=3;
mean(Y) = (3 + 5 + 7 + 9 + 11) / 5 = 7
然后计算出X和Y的协方差。协方差定义为变量之间的乘积与其均值之差的期待值。
Cov(X, Y) = E((X - mean(X))(Y - mean(Y)))
将我们的数据代入计算：
Cov(X, Y) = ((1-3)(3-7) + (2-3)(5-7) + (3-3)(7-7) + (4-3)(9-7) + (5-3)(11-7)) / 5
= (-4 + 2 + 0 + 2 + 4) / 5 = 0
我们可以得到协方差矩阵：
Σ = [[Var(X), Cov(X, Y)],
[Cov(Y, X), Var(Y)]]
在这个例子中，由于X和Y之间的协方差为0，表示它们之间没有线性相关性。协方差矩阵如下：

Σ = [[Var(X), 0],
[0, Var(Y)]]
协方差矩阵的对角线元素表示各个变量的方差，即Var(X)和Var(Y)。在这个例子中，我们可以计算出：

Var(X) = ((1-3)^2 + (2-3)^2 + (3-3)^2 + (4-3)^2 + (5-3)^2) / 5 = 2
Var(Y) = ((3-7)^2 + (5-7)^2 + (7-7)^2 + (9-7)^2 + (11-7)^2) / 5 = 8
所以协方差矩阵为：

Σ = [[2, 0],
[0, 8]]

通过协方差矩阵，我们可以得出X和Y的方差以及它们之间的协方差。这些统计量可以帮助我们分析
变量之间的关系和变量的分布情况。

过程噪声协方差矩阵Q：用于描述系统模型中的过程噪声的性质。卡尔曼滤波器是一种估计状态的算法，
它基于系统模型和测量数据进行计算。系统模型描述了系统的动态行为，而测量数据提供了关于
系统状态的观测值。然而，由于系统模型无法完全准确地描述系统行为，以及测量数据中的噪声，
卡尔曼滤波器引入了过程噪声协方差矩阵来模拟这些不确定性。
过程噪声协方差矩阵Q通常是一个正定对称矩阵。它的元素描述了不同状态变量之间的相关性以及系统
在不同时间步长上的不确定性。在卡尔曼滤波预测步骤中，Q用于更新系统状态的协方差矩阵，以
反应模型中的不确定性。通过不断地调整Q的大小，可以平衡系统模型和测量数据的信任程度，
从而实现更准确的状态估计。
总而言之，过程噪声协方差矩阵Q用于描述系统模型中的不确定性和噪声，并在卡尔曼滤波器中
用于更新状态估计的协方差。作用：提供对系统状态的最优估计，并在估计过程考虑不确定性。
例：假设一个移动机器人，通过激光雷达对周围进行感知和定位。使用卡尔曼滤波器来估计
机器人的位置和速度。
在系统模型中，我们假设机器人的位置和速度服从一个线性动力学模型，即：
x(k) = Fx(k-1) + Bu(k) + w(k)
其中，x(k)是机器人的状态向量，F是状态转移矩阵，B是输入矩阵，u(k)是输入向量，w(k)是过程噪声
过程噪声通常表示了系统模型中的不确定性和外部干扰。如：机器人在移动过程中可能会
受到风的影响或者地面摩擦力的变化等。这些因素都会导致机器人的移动轨迹与理论模型有偏差。
为了将这些不确定性考虑进去，我们引入过程噪声协方差矩阵Q。Q的大小和分布描述了过程噪声
的性质。较大的Q表示系统模型中的不确定性较大，较小的Q表示系统模型的预测能力较强。
我们使用Q来更新状态向量的协方差矩阵P。P表示对机器人状态估计的不确定性。通过
更新P，我们能够更好地反映系统模型中的不确定性，并在状态估计中进行更准确的预测。

观察噪声协方差矩阵R：用于描述观测值的噪声影响。在许多实际问题中，我们无法完全
准确地测量或观察到真实值，而是受到噪声的影响。这些噪声可能来自测量仪器的误差，环境干扰等、
观察噪声协方差矩阵R可以帮我们量化这种噪声的影响。它是一个对称正定矩阵，用于描述
噪声在不同维度上的方差和协方差。
在卡尔曼滤波估计等问题中，观测噪声协方差矩阵R通常用于计算卡尔曼增益，从而根据观测值
和先验估计值来融合信息，得到更准确的估计值。R
的值越大，表示观测噪声越大，对估计结果的影响也越大。
注意：观测噪声协方差矩阵R是一个经验参数，需要根据具体问题和实验数据进行估计或调整。通常，
我们通过实验或领域知识来估计R的值。

状态协方差矩阵P：是卡尔曼滤波算法中用于描述对系统状态估计的不确定的矩阵。它表示状态向量
各个状态变量之间以及同一个状态变量在不同时刻之间的协方差关系。
例：假设一个简单系统，其状态向量包含两个状态变量：位置和速度，用x表示位置，v表示速度，则
状态向量为X=[x;v];
状态协方差矩阵是一个与状态向量维度一致的对称矩阵，它表示了位置和速度之间以及同一个状态变量
在不同时刻之间的协方差关系，假设状态协方差矩阵为P，则P的元素可以表示为：
P = [Pxx Pxv;
Pvx Pvv] 其中，Pxx表示位置x的方差，Pvv表示速度v的方差，Pxv和Pvx表示位置x和速度v之间的协方差。
例：我们考虑一个移动的物体，通过传感器测量物体的位置和速度。初始时刻，我们对物体的位置和速度
没有任何信息，因此可以将状态协方差矩阵P初始化为一个较大的值，表示对状态估计不确定较高。
随着时间的推移，我们通过状态转移方程和测量方程，不断更新状态估计和状态协方差矩阵。
例：在一个时间步骤中，根据物体的运动模型和先前的状态估计，可以预测物体下一时刻的
位置和速度，并更新状态协方差矩阵。
在更新状态协方差矩阵时，如果物体在该时间被传感器测量到了位置和速度的值，我们可以使用
测量结果来修正状态协方差矩阵。通过卡尔曼增益的计算。。。。

一 .入门
1.引入
2.适用系统：线性高斯系统
线性：y=kx+b  z=ax+by  w=u^2
满足（叠加性和其次性）
1.叠加性 y=ax1+bx2;
2.其次性 kx-->ky

高斯：噪声满足正态分布

3.宏观意义：滤波即加权
理想状态：信号*1+噪声*0；
                   低频 1  高频0    低通滤波
                   估计（） 观察()  卡尔曼滤波
                        修正


二.进阶
1.状态空间表达式