无人机高度估计_卡尔曼滤波

一、 卡尔曼滤波公式

  1. 线性系统的离散状态空间表达式:这是卡尔曼五个标准公式的基础

    ①式叫做系统状态方程:
    xk: k时刻系统状态变量值
    Ak:状态转移矩阵
    Bk: 控制矩阵
    uk-1: k-1时刻系统的输入
    wk-1: k-1时刻的过程噪声,服从高斯分布,均值为0,协方差为Q
    ②式叫做观测方程:与传感器测量值有关
    zk: k时刻的观测值/测量值;
    Hk: 观测矩阵
    vk: k时刻的测量噪声,服从高斯分布,均值为0,协方差为R

  2. 卡尔曼滤波的五个标准公式:分为时间更新方程和测量更新方程
    2.1 时间更新方程(2个方程)

    ①式等式左边的符号表示 xk的先验估计,即xk的估计值的预测值
    ②式等式坐标的符号表示误差协方差矩阵的先验估计,Q表示过程噪声协方差矩阵

2.2 测量更新方程(3个方程)

①式:Kk表示卡尔曼增益。Kk趋近0时,②式的结果更相信估计值;Kk趋近1时,更相信测量值。实际调参过程中调的是Q(过程噪声协方差矩阵)和R(测量噪声协方差矩阵),然后Q,R 值影响了Kk。Q越大,表示越不相信估计值,R越大表示越不相信测量值。
②式:状态的后验估计。这就是卡尔曼滤波的最终的结果。里面包含和估计值的预测值和观测值。
③式:误差协方差矩阵的后验估计。

二、 利用卡尔曼滤波估计高

这里使用的气压计(SPL06)和加速度计(MPU9250)进行融合。这里忽略了噪声。
步骤1:明确几个变量
系统的输入变量uk : 地球系下的加速度。
系统状态变量(2个)Xk: Pk,表示位置,这里指的是高度;Vk,表示速度变量;更明确的来说是地球系下Z轴的位移和Z轴的速度。
系统观测变量: Zk,表示气压计测量到的高度。这里要转换到地球系。
步骤2:确定系统的离散状态空间表达式,然后根据表达式写出矩阵方程,最后就得到了A,H,B

三、 实战:移植到MiniFly微型四轴上面

MiniFly是正点原子的开源微型四轴,使用的是stm32F4系列MCU,上面搭载了气压计,可以进行定高。
MiniFly高度估计代码的位置:
Firmware_F411 V1.4(改灯)\FLIGHT\src\state_estimator.c

传入的参数是Z轴加速度和气压计高度(如果接上了光流模块,使用的是光流模块上面的测距仪测到的高度)。因此我们可以使用自己些的kalman滤波高度估计算法,替代原来代码的实现。

四、卡尔曼滤波的调参

kalman调的参数只有两个Q和R。增大Q表示越不信任估计量,增大R表示越不信任观测量。在卡尔曼滤波高度估计中,设置R=1,Q=4e-8f,效果较好。

五、卡尔曼滤波实现高度估计的代码

代码中的矩阵运算使用的是STM32提供的DSP_LIB

/*
	dt:传递给A和B的时间  
    acc: kalman滤波的输入1
    atti: kalman滤波的输入2
	Q:	根据经验给定过程噪声协方差矩阵,这里是一个2X2矩阵
	R:	根据经验给定测量噪声协方差矩阵 
	pos_esti: kalman滤波结果输出1
	vel_esti: kalman滤波结果输出2
*/
void KalmanPosZEsti(float dt, 	// 时间间隔
					float accz,		// 地球系下,Z轴加速度 (m/s^2)
					float atti,		// 地球系下,气压计(或测距仪)测量的高度 (m)
					const float R,
					const float Q,
					float *pos_esti, // 位置估计
					float *vel_esti)
{
	 /*定义保存矩阵A,B值的一维数组*/
	 static float A[4] = { 0 }; //一维数组
	 static float B[2] = { 0 };
	 static float H[2] = { 0 };
	 static float HPHT[1] = {0} ; // 特例
	 static float HT[2] = { 0 };
	 static float Q_[4] = {0};
	 
	 static float X_[2] = {0,0}; // 状态变量的先验估计初始值为{0,0}
	 static float X[2] = {0.0f,0.0f}; // 状态变量的后验估计初始值为{0,0}
	 static float P_[4] = {0,0,0,0}; // 误差协方差矩阵先验的初始值为{0,0,0,0}
	 static float P[4] = {1.0,0,0,1.0}; // 误差协方差矩阵后验的初始值为{1,0,0,1}
	 static float AX[2] = {0,0};
	 static float AHATP[4] = {0};
	 static float AHATPAT[4] = {0};
	 static float PHT[2] = { 0 };
	 static float KH[4] = {0};
	 
	 static float32_t K[2] = {0};
	 static float HK[2] = { 0 };
	 static float I_KH[2] = { 0 };
	 static float I[4] = {1,0,0,1};
	 
	 static float u = 0; // 定义控制变量,也就是系统的输入
	 
	 // 定义矩阵实例
	 arm_matrix_instance_f32 _A; // 定义一个矩阵实例 _A,保存一维数组A的值
	 arm_matrix_instance_f32 _A_T; // 定义一个矩阵实例 _A_T ,保存一维数组A的值
	 arm_matrix_instance_f32 _B; // 定义一个矩阵实例 _B,保存一维数组B的值
	 arm_matrix_instance_f32 _H; // 定义一个矩阵实例 _H,保存一维数组H的值
	 arm_matrix_instance_f32 _HT; // 定义一个矩阵实例 _HT,保存一维数组H的值
	 arm_matrix_instance_f32 hatx_; // Xk的先验估计
	 arm_matrix_instance_f32 hatx; // Xk的后验估计
	 arm_matrix_instance_f32 hatP_; // P的先验估计 hatP_
	 arm_matrix_instance_f32 hatP; // P的后验估计 hatP
	 arm_matrix_instance_f32 Kk; // kalman增益
	 arm_matrix_instance_f32 _KH;
	 arm_matrix_instance_f32 _I_KH;
	 arm_matrix_instance_f32 _I;
	 
	 // 临时的中间矩阵
	 arm_matrix_instance_f32 A_hatX; // Ak * hatXk-1
	 arm_matrix_instance_f32 A_hatP; // Ak * hatPk-1
	 arm_matrix_instance_f32 A_hatPAT; // Ak * hatPk-1 * AT
	 arm_matrix_instance_f32 HkhatPk_; 
	 arm_matrix_instance_f32 hatPk_HkT; // hatPk_ * HkT
	 arm_matrix_instance_f32 HP_HT; //H*P_*H'
	 
	 arm_matrix_instance_f32 _Q;
	 Q_[0] = Q;
	 Q_[1] = 0;
	 Q_[2] = 0;
	 Q_[3] = Q;
	 arm_mat_init_f32(&_Q, 2, 2, Q_); // _A 矩阵是2X2的矩阵,里面的值是A
	 	 
	 u = accz; // 将加速度
	 
	 A[0] = 1; A[1] = dt;
	 A[2] = 0; A[3] = 1;
	 
	 B[0] = 0.5*dt*dt * u;
	 B[1] = dt * u;
	 
	 H[0] = 1;
	 H[1] = 0;
	 
	 // 初始化矩阵实例
	 arm_mat_init_f32(&_A, 2, 2, A); // _A 矩阵是2X2的矩阵,里面的值是A
	 arm_mat_init_f32(&_A_T, 2, 2, A); // _A_T 矩阵是2X2的矩阵,里面的值是A
	 arm_mat_init_f32(&_B, 2, 1, B); // _B 矩阵是2X1的矩阵,里面的值是B
	 arm_mat_init_f32(&_H, 1, 2, H); // _H 矩阵是1X2的矩阵,里面的值是H
	 arm_mat_init_f32(&_HT, 2, 1, H); // _HT 矩阵是2X1的矩阵,里面的值是H
	 arm_mat_init_f32(&HP_HT, 1, 1, HPHT); //
	 arm_mat_init_f32(&hatx_, 2, 1, X_); // hatx_ 矩阵是2X1的矩阵,里面的值是X_
     arm_mat_init_f32(&hatx, 2, 1, X); // hatx 矩阵是2X1的矩阵,里面的值是X
	 arm_mat_init_f32(&A_hatX, 2, 1, AX); // A_hatX 矩阵是2X1的矩阵,里面的值是AX
	 arm_mat_init_f32(&hatP_, 2, 2, P_); // hatP_ 矩阵是2X2的矩阵,里面的值是P_
	 arm_mat_init_f32(&hatP, 2, 2, P); // hatP 矩阵是2X2的矩阵,里面的值是P
	 arm_mat_init_f32(&A_hatP, 2, 2, AHATP);  
	 arm_mat_init_f32(&A_hatPAT, 2, 2, AHATPAT);  
	 arm_mat_init_f32(&Kk, 2, 1, K);
	 arm_mat_init_f32(&hatPk_HkT, 2, 1, PHT);
	 arm_mat_init_f32(&HkhatPk_, 1, 2, HK);
	 arm_mat_init_f32(&_KH, 2, 2, KH);
	 arm_mat_init_f32(&_I_KH, 2, 2, I_KH);
	 arm_mat_init_f32(&_I, 2, 2, I);
	 
	 //kalman公式1: xk的先验估计 
	 arm_mat_mult_f32(&_A, &hatx, &A_hatX); //  A_hatX = _A * hatx
	 arm_mat_add_f32(&A_hatX, &_B, &hatx_); // 卡尔曼滤波公式1,hatx_是先验估计
	 
	 //kalman公式2:误差协方差矩阵的先验估计
	 arm_mat_trans_f32(&_A, &_A_T);					// A' (2x2)
	 arm_mat_mult_f32(&_A, &hatP, &A_hatP); //  A_hatP = _A * hatP
	 arm_mat_mult_f32(&A_hatP, &_A_T, &A_hatPAT); //  A_hatPAT = A_hatP * _A_T
	 arm_mat_add_f32(&A_hatPAT, &_Q, &hatP_); // hatP_ = A_hatPAT + Q
	 
	 // kalman公式3:Kk = hatPk_ * HkT*(Hk*hatPk_HT + R)’
	 arm_mat_mult_f32(&_H, &hatP_, &HkhatPk_);
	 arm_mat_trans_f32(&_H, &_HT);					
	 arm_mat_mult_f32(&HkhatPk_, &_HT, &HP_HT);		
	 HPHT[0] += R;															
	 HPHT[0] = 1 / HPHT[0];											
	 arm_mat_mult_f32(&hatP_, &_HT, &hatPk_HkT);		
	 arm_mat_mult_f32(&hatPk_HkT, &HP_HT, &Kk);	
	 
	 
	 // 卡尔曼公式4
	 arm_mat_mult_f32(&_H, &hatx_, &HP_HT);	
	 HPHT[0] = atti - HPHT[0];							
	 arm_mat_mult_f32(&Kk, &HP_HT, &A_hatX);	
	 arm_mat_add_f32(&hatx_, &A_hatX, &hatx);	

	 // 卡尔曼公式5
	 arm_mat_mult_f32(&Kk, &_H, &_KH);		
	 arm_mat_sub_f32(&_I, &_KH, &_I_KH);			
	 arm_mat_mult_f32(&_I_KH, &hatP_, &hatP);		
	 
	 *pos_esti =  hatx.pData[0];// 位置估计
	 *vel_esti =  hatx.pData[1];
}

文章出处登录后可见!

已经登录?立即刷新

共计人评分,平均

到目前为止还没有投票!成为第一位评论此文章。

(0)
xiaoxingxing的头像xiaoxingxing管理团队
上一篇 2022年5月26日
下一篇 2022年5月26日

相关推荐