使用加速度传感器与GPS位移求速度

根据卡尔曼滤波来进行计算，刚开始时，不清楚为什么卡尔曼滤波能够预测物体的状态，看了几天后发现，采样率是很重要的一个点。

假设车辆做匀加速直线运动，要预测车辆的速度vt，在采样率确定的情况下，很明显时间t就成为了已知量,假如位移s可以测出来，那么就可以求平均速度v_，如果加速度a也可以测出来，那就可以预测瞬时速度vt。
v_ = s / t
vt = v0+at

针对于某一时刻，它的前一时刻速度是已知的(每一次采样更新速度后进行保存)，变化的时间是已知的，加速度也是已知的，所以可以计算出当前时刻的速度。

以下是例子：
python

# -*- coding: utf-8 -*
import numpy as np
import matplotlib.pyplot as plt
 
def main():
    # 时间共1s，采样周期10ms
    dt = 0.01
    t = [i * dt for i in range(0, 1000)]
 
    g = 9.8
 
    # 真实值
    x_true_mat = np.mat(0.5 * g * np.multiply(np.array(t), np.array(t)))
    v_true_mat = g * np.mat(t)
    u_true_mat = np.mat([g for i in range(0, 1000)])
 
    # 噪声
    x_noise = np.round(np.random.normal(0, 0.1, 1000), 2)
    v_noise = np.round(np.random.normal(0, 0.1, 1000), 2)
    u_noise = np.round(np.random.normal(0, 0.01, 1000), 2)
    x_noise_mat = np.mat(x_noise)
    v_noise_mat = np.mat(v_noise)
    u_noise_mat = np.mat(u_noise)
 
    # 测量值
    x_z_mat = x_true_mat + x_noise_mat
    v_z_mat = v_true_mat + v_noise_mat
 
    u_mat = u_true_mat + u_noise_mat
 
    # 定义x的初始状态
    x_mat = np.mat([[0], [0]])
    # 定义初始状态协方差矩阵
    p_mat = np.mat([[1, 0], [0, 1]])
    # 状态转移矩阵
    f_mat = np.mat([[1, dt], [0, 1]])
    # 控制矩阵
    b_mat = np.mat([[0.5 * dt * dt], [dt]])
    # 定义状态转移协方差矩阵，这里我们把协方差设置的很小，因为觉得状态转移矩阵准确度高
    q_mat = np.mat([[1.0 * 1.0 * dt * dt, 0], [0, 1.0 * 1.0 * dt * dt]])
    # 定义观测矩阵
    h_mat = np.mat([[1,0]])
    # 定义观测噪声协方差
    r_mat = 5

    I_mat = np.mat([[1, 0], [0, 1]])
    count = 0
    for i in range(1000): 

        x_predict = f_mat * x_mat + b_mat * u_mat[0, i]
        p_predict = f_mat * p_mat * f_mat.T + q_mat

        if count%100 == 0: #1秒时间到,GPS位移更新
            k = p_predict * h_mat.T * (h_mat * p_predict * h_mat.T + r_mat).I
            zt = x_z_mat[0, i]
            x_mat = x_predict + k * (zt - h_mat * x_predict )
        else:
            x_mat = x_predict
        p_mat = (I_mat - k * h_mat) * p_predict

        plt.plot(t[i], x_z_mat[0, i], 'ro', markersize=1)
        plt.plot(t[i], v_z_mat[0, i], 'ro', markersize=1)
        plt.plot(t[i], x_mat[0, 0], 'bo', markersize=1)
        plt.plot(t[i], x_mat[1, 0], 'bo', markersize=1)
        count+=1
 
    plt.show()
 
 
if __name__ == '__main__':
    main()

C语言 卡尔曼计算

void Kalman_Fil_Calc(KalFil_t *kf, double acc, double distance, int dis_update)
{
    // 中间变量
    double tempB[2][1] = {{0.5 * Period * Period}, {Period}};
    double X1[2][1] = {0};
    double X2[2][1] = {0};
    double P1[2][2] = {0};
    double P2[2][2] = {0};
    double K1[2][1] = {0};
    double K2[1][2] = {0};
    double K3 = 0;
    double X3 = 0;
    double K4[2][1] = {0};
    double P3[2][2] = {0};

    kf->A[0][0] = 1;
    kf->A[0][1] = Period; //dt
    kf->A[1][0] = 0;
    kf->A[1][1] = 1;

    kf->At[0][0] = 1;
    kf->At[0][1] = 0;
    kf->At[1][0] = Period;
    kf->At[1][1] = 1;

    // 下一状态估计
    matrix_multiply((double *)kf->A, (double *)kf->X, (double *)X1, 2, 2, 1); // X1 = A*x(k-1)
    kf->B[0][0] = tempB[0][0] * acc;                                          // B*u(k)
    kf->B[1][0] = tempB[1][0] * acc;
    matrix_plus((double *)X1, (double *)kf->B, (double *)X2, 2, 1); // X2 = A*x(k-1)+B*u(k)

    // 计算协方差矩阵
    matrix_multiply((double *)kf->A, (double *)kf->P, (double *)P1, 2, 2, 2); // P1 = A*kf->P
    matrix_multiply((double *)P1, (double *)kf->At, (double *)P2, 2, 2, 2);   // P2 = P1*kf->At
    matrix_plus((double *)kf->Q, (double *)P2, (double *)P1, 2, 2);           // P1 = P2 + Q

    if (dis_update == 1)
    {        
        
        printf("X2:\n%f\n%f\n\n",X2[0][0],X2[1][0]);

        // 计算卡尔曼增益矩阵
        matrix_multiply((double *)P1, (double *)kf->Ht, (double *)K1, 2, 2, 1); // K1 = P1*Ht
        matrix_multiply((double *)kf->H, (double *)P1, (double *)K2, 1, 2, 2);  // K2 = H*P1
        K3 = K2[0][0] * kf->Ht[0][0] + K2[0][1] * kf->Ht[1][0];                 // K3 = H*P1*Ht
        double tempK = 1 / (K3 + kf->R);                                        //1/[H*P1*Ht+R]
        kf->K[0][0] = K1[0][0] * tempK;                                         //K = P1*Ht*(1/[H*P1*Ht+R])
        // kf->K[1][0] = K1[1][0] * tempK;

        // 计算状态最优估计
        X3 = kf->H[0][0] * X2[0][0] + kf->H[0][1] * X2[1][0];                    // X3 = H * X2 
        double tempX = distance - X3;                                            // Z- (H * X2) 
        K4[0][0] = kf->K[0][0] * tempX;                                          //K*(Z- (H * X2))
        K4[1][0] = kf->K[1][0] * tempX;
        matrix_plus((double *)X2, (double *)K4, (double *)kf->X, 2, 2);          //X = X2+ K*(Z- (H * X2))

    }
    else
    {
        kf->X[0][0] = X2[0][0];
        kf->X[1][0] = X2[1][0];
    }

    // 更新协方差矩阵
    matrix_multiply((double *)kf->K, (double *)kf->H, (double *)P3, 2, 1, 2); // P3 = K*H
    matrix_minus((double *)kf->I, (double *)P3, (double *)P2, 2, 2);          // P2 = I - K*H
    matrix_multiply((double *)P2, (double *)P1, (double *)kf->P, 2, 2, 2);    // P = (I - K*H)*P1
}

C语言完整版
使用加速度传感器与GPS位移求速度

参考
IMU和GPS数据融合估计位置与速度（MATLAB实现
使用卡尔曼滤波估计自由落体时的位置和速度
基于加速度计与气压计的三阶卡尔曼滤波计算加速度、速度及高度