Kalman滤波

Kalman滤波是什么及作用

一种通过预测值和测量值组合来滤除随机噪音和不确定性，得到当前状态的滤波方法。
可用于融合多个传感器、估算带噪音的数据。

例如一个机器人可以沿着X轴运动，在理论上，若以10m/s的速度从0点出发向正方向运动，该机器人在1s过后将位于10m处。
但在实际情况中，由于轮子与地面的滑移和障碍物减缓速度的情况随机发生，1s的运动后机器人的位置是一个正态分布，其均值u=10，σ与机器人控制精度有关。
与此同时，若使用传感器来测量机器人的位置，得到的结果通常也是正态分布的。

正态分布及求和计算
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
#include <iostream>
#include <math.h>

using namespace std;

double f(double mu, double sigma2, double x)
{
    //Use mu, sigma2 (sigma squared), and x to code the 1-dimensional Gaussian
    //Put your code here
    double prob = 1.0 / sqrt(2.0 * M_PI * sigma2) * exp(-0.5 * pow((x - mu), 2.0) / sigma2);
    return prob;
}

int main()
{
    double sum = 0.0;
    for (int i = 0; i != 20; ++i) {
        double x_val = 0.0 + double(i);
        double gaussian = f(5.0, 4.0, x_val);
        cout << gaussian << endl;

        sum += gaussian;
    }
    //cout << f(10.0, 4.0, 8.0) << endl;

    cout << "sum is " << sum << endl; // 接近1，类似于高斯的概率密度函数积分为1
    return 0;
}
命名惯例

x_t 状态 state
z_t 测量 measurement
u_t 控制 control action

测量更新 measurement update

例如，给机器人一个控制命令后，我们认为其位置（Prior belief）的高斯分布为mu,sigma2。
若此时，传感器测量得到的位置分布为v,r2，
那么构建融合的高斯分布是这样的：
新的均值：mu’ = (r2mu + sigma2v)/(r2 + sigma2)
新的方差：sigma2’ = 1/(1/r2 + 1/sigma2)

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
#include <iostream>
#include <math.h>
#include <tuple>

using namespace std;

double new_mean, new_var;

tuple<double, double> measurement_update(double mean1, double var1, double mean2, double var2)
{
    new_mean = (var2 * mean1 + var1 * mean2) / (var1 + var2);
    new_var = 1 / (1 / var1 + 1 / var2);
    return make_tuple(new_mean, new_var);
}

int main()
{
    tie(new_mean, new_var) = measurement_update(10, 8, 13, 2);
    printf("[%f, %f]", new_mean, new_var);
    return 0;
}
状态预测

在知道当前先验位置分布mu1,sigma2(1)的时候（sigma2表示sigma的平方），给出运动控制指令的先验作用效果分布mu2,sigma2(2),
预测其状态的公式如下：
后验均值 Posterior Mean mu’ = mu1 + mu2
后验方差 Posterior Variance sigma2’ = sigma2(1) + sigma2(2)

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
#include <iostream>
#include <math.h>
#include <tuple>

using namespace std;

double new_mean, new_var;

tuple<double, double> state_prediction(double mean1, double var1, double mean2, double var2)
{
    new_mean = mean1 + mean2;
    new_var =  var1 + var2;
    return make_tuple(new_mean, new_var);
}

int main()
{
    tie(new_mean, new_var) = state_prediction(10, 4, 12, 4);
    printf("[%f, %f]", new_mean, new_var);
    return 0;
}
1D Kalman 滤波代码
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
#include <iostream>
#include <math.h>
#include <tuple>

using namespace std;

double new_mean, new_var;

tuple<double, double> measurement_update(double mean1, double var1, double mean2, double var2)
{
    new_mean = (var2 * mean1 + var1 * mean2) / (var1 + var2);
    new_var = 1 / (1 / var1 + 1 / var2);
    return make_tuple(new_mean, new_var);
}

tuple<double, double> state_prediction(double mean1, double var1, double mean2, double var2)
{
    new_mean = mean1 + mean2;
    new_var = var1 + var2;
    return make_tuple(new_mean, new_var);
}

int main()
{
    //Measurements and measurement variance
    double measurements[5] = { 5, 6, 7, 9, 10 };
    double measurement_sig = 4;
    
    //Motions and motion variance
    double motion[5] = { 1, 1, 2, 1, 1 };
    double motion_sig = 2;
    
    //Initial state
    double mu = 0;
    double sig = 1000;
    
    //######TODO: Put your code here below this line######//
    // Loop through all the measurments
    for (int i = 0; i < sizeof(measurements)/sizeof(measurements[0]); i++){
        // Apply a measurment update
        tie(mu, sig) = measurement_update(mu, sig, measurements[i], measurement_sig);
        printf("update:  [%f, %f]\n", mu, sig);
        // Apply a state prediction
        tie(mu, sig) = state_prediction(mu, sig, motion[i], motion_sig);
        printf("predict: [%f, %f]\n", mu, sig);
    }
    return 0;
}

多维度卡尔曼滤波

2d高斯

均值：一个均值组成的向量
方差：协方差矩阵，其对角线为方差，二维图像上与椭圆的长短轴相关；对角线之外的是相关项，与椭圆转动有关，正值向右转。

维度中变量的组合可是位置组合如[x, y]’,也可以是位置与速度组合如[x, 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
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
#include <iostream>
#include <math.h>
#include <tuple>
#include "eigen/eigen/Core" // Eigen Library
#include "eigen/eigen/LU"   // Eigen Library

using namespace std;
using namespace Eigen;

float measurements[3] = { 1, 2, 3 };

tuple<MatrixXf, MatrixXf> kalman_filter(MatrixXf x, MatrixXf P, MatrixXf u, MatrixXf F, MatrixXf H, MatrixXf R, MatrixXf I)
{
    for (int n = 0; n < sizeof(measurements) / sizeof(measurements[0]); n++) {

        // Measurement Update
        MatrixXf Z(1, 1);
        Z << measurements[n];

        MatrixXf y(1, 1);
        y << Z - (H * x);

        MatrixXf S(1, 1);
        S << H * P * H.transpose() + R;

        MatrixXf K(2, 1);
        K << P * H.transpose() * S.inverse();

        x << x + (K * y);

        P << (I - (K * H)) * P;

        // Prediction
        x << (F * x) + u;
        P << F * P * F.transpose();
    }

    return make_tuple(x, P);
}

int main()
{

    MatrixXf x(2, 1);// Initial state (location and velocity) 
    x << 0,
    	 0; 
    MatrixXf P(2, 2);//Initial Uncertainty
    P << 100, 0, 
    	 0, 100; 
    MatrixXf u(2, 1);// External Motion
    u << 0,
    	 0; 
    MatrixXf F(2, 2);//Next State Function
    F << 1, 1,
    	 0, 1; 
    MatrixXf H(1, 2);//Measurement Function
    H << 1,
    	 0; 
    MatrixXf R(1, 1); //Measurement Uncertainty
    R << 1;
    MatrixXf I(2, 2);// Identity Matrix
    I << 1, 0,
    	 0, 1; 

    tie(x, P) = kalman_filter(x, P, u, F, H, R, I);
    cout << "x= " << x << endl;
    cout << "P= " << P << endl;

    return 0;
}

Kalman滤波的类型

KF-线性
EKF-非线性
UKF-高度非线性