根据卡尔曼滤波来进行计算,刚开始时,不清楚为什么卡尔曼滤波能够预测物体的状态,看了几天后发现,采样率是很重要的一个点。
假设车辆做匀加速直线运动,要预测车辆的速度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实现
使用卡尔曼滤波估计自由落体时的位置和速度
基于加速度计与气压计的三阶卡尔曼滤波计算加速度、速度及高度