KF与EKF代码demo（几分钟就能让人人都会使用）

本文中代码均为C#实现，可自行换为其他语言。

1.KF，对数据自身或者有对应观测值的数据进行滤波（一维线性）

namespace ZaneCon.PredictedState.FeederState
{
    //本次卡尔曼针对的状态量与观测值均为特定情境下一维线性
    public class FeederKF
    {
        private double A; // 状态转移矩阵
        private double H; // 观测矩阵
        private double Q; // 状态噪声协方差矩阵
        private double R; // 观测噪声协方差矩阵
        private double P; // 状态估计协方差矩阵
        private double x; // 状态估计向量
        public FeederKF()
        {
            // 初始化矩阵和向量
  //因为一维线性，比如模型为y=2x或直接对数据过滤（没有线性关系，一组数据或两组数据——一组状态量+一组观测）
            A = 1;
            H = 1;
            Q = 0.1;
            R = 1;
            P = 1;
            x = 0;
        }
       //这里x为状态量，z为对应的观测数据
        public double Filter(double x, double z)
        {
            // 预测步骤
            double x_ = A * x;
            double P_ = A * P * A + Q;
            // 更新步骤
            double K = P_ * H / (H * P_ * H + R);
            x = x_ + K * (z - H * x_);
            P = (1 - K * H) * P_;
            return x;
        }
    }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36

上述例子应用对象为两组数据进行融合滤波，比如测得A组数据为一系列值——数组A，A组观测数据为一系列值——数组B，现融合A和B得到更准的A数据。

2.EKF，与KF相比就是A阵不同，一般对非线性模型进行线性化，其他步骤一模一样。

namespace ZaneCon.PredictedState.FeederState
{
    //EKF完整例子，用时修改状态量、量测量、维数及A阵即可
    public class FeederEKF
    {
        // 状态转移矩阵
        private Matrix<double> A; //状态转移矩阵
        private Matrix<double> H; // 观测矩阵
        private Matrix<double> Q; // 状态噪声协方差矩阵(肯定与状态量维数一样)
        private Matrix<double> R; // 观测噪声协方差矩阵（肯定与观测量维数一样）
        private Matrix<double> P; // 状态估计协方差矩阵
        private Matrix<double> X; // 状态估计向量6维，量测2维

        public FeederEKF()
        {
            // 初始化矩阵和向量
            A = DenseMatrix.CreateIdentity(6);//6*6的单位阵,状态维度*状态维度
            H = Matrix<double>.Build.DenseOfArray(new double[,] { { 1, 0, 0, 0, 0, 0 }, { 0, 1, 0, 0, 0, 0 } });//2*6,量测维度*状态维度
            Q = DenseMatrix.CreateIdentity(6);//6*6状态噪声协方差矩阵,状态维度*状态维度
            R = Matrix<double>.Build.DenseOfArray(new double[,] { { 1, 0 }, { 0, 1 } });//2*2观测噪声协方差矩阵,量测维度*量测维度
            // 状态估计协方差矩阵,状态维度*状态维度
            P = Matrix<double>.Build.DenseOfArray(new double[,] { { 10, 0, 0, 0, 0, 0 }, { 0, 10, 0, 0, 0, 0 }, { 0, 0, 10, 0, 0, 0 }, { 0, 0, 0, 10, 0, 0 }, { 0, 0, 0, 0, 10, 0 }, { 0, 0, 0, 0, 0, 10 } });
            X = Matrix<double>.Build.Dense(6, 1, 0.0); ;//6*1
        }
      //此处状态量为x、y、thta、v、w、a，观测量为z_x、z_y
        public Matrix<double> Filter(double x, double y, double thta, double v, double w, double a, double delta_t, double z_x, double z_y)
        {
            /* 此处状态量及其机械编排(离散化)为:x=x+v*Cos(thta);
                         y=y+v*Sin(thta);
                         thta=thta+wt;
                         v=v+at;
                         w
                         a
               量测为x,y;
             */
            A[0, 2] = -v * Asin(thta);//thta必须在-1~1之间，不然出现NAN
            A[0, 3] = Acos(thta);
            A[1, 2] = v * Acos(thta);
            A[1, 3] = Asin(thta);
            A[3, 4] = delta_t;
            A[4, 5] = delta_t;
            Matrix<double> Z = Matrix<double>.Build.DenseOfArray(new double[,] { { z_x }, { z_y } });//量测阵
            X = Matrix<double>.Build.DenseOfArray(new double[,] { { x }, { y }, { thta }, { v }, { w }, { a } });
            // 预测步骤
            Matrix<double> X_ = A * X;
            Matrix<double> P_ = A * P * A.Transpose() + Q;
            // 更新步骤
            Matrix<double> K = (P_ * H.Transpose()) * (H * P_ * H.Transpose() + R).Inverse();
            X = X_ + K * (Z - H * X_);
            P = (1 - K * H) * P_;
            return X;
        }
    }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54

使用EKF一定要注意各个阵的维度设置，另外A阵雅可比的计算依赖于机械编排（模型式子求偏导），至于雅可比矩阵的排列可以不懂的可以百度一样，很简单。A阵求出来后，直接套用公式即可，与KF一样。
此处的例子是通过状态量间接算出测量值xy，然后与观测值xy进行融合滤波，得到更精确的xy。比如通过imu计算得到的xy与GPS得到的xy进行融合。

通过上述demo，现在是不是觉得晦涩难懂的理论在应用起来却是非常简单呢？