STM32应用(三)一阶卡尔曼滤波原理和ADC读取红外测距模块的数值

1 一阶卡尔曼滤波

如何通俗并尽可能详细地解释卡尔曼滤波？
图说卡尔曼滤波，一份通俗易懂的教程

1.0 宏观展示

卡尔曼滤波器可以分为时间更新方程和测量更新方程。时间更新方程（即预测阶段）根据前一时刻的状态估计值推算当前时刻的状态变量先验估计值和误差协方差先验估计值；测量更新方程（即更新阶段）负责将先验估计和新的测量变量结合起来构造改进的后验估计。时间更新方程和测量更新方程也被称为预测方程和校正方程。因此卡尔曼算法是一个递归的预测——校正方法。

1.1 预测状态方程

预测状态方程参数：

^X_k-1	上一时刻( k-1 时刻) 的后验估计值（初始化赋值）
^X_k`	当前时刻( k 时刻) 的状态，得到 k 时刻的先验估计值
u_k	输入量（或者说控制增益）
A	状态转移矩阵，实际上是对目标状态转换的一种猜想模型
B	将输入转换为状态的矩阵

方程含义： 位置最优值的预测。

由 系统状态变量k-1时刻的最优值 和 系统输入 计算出 k时刻的系统状态变量k时刻的最优值。

1.2 预测协方差方程

预测协方差方程参数：

P_k-1	k-1 时刻的后验估计协方差
^P_k`	k 时刻的先验估计协方差
Q	过程激励噪声协方差（用来表示表示状态转换矩阵与实际过程之间的误差）

方程含义： 预测位置最优值的方差。

根据 k-1时刻的系统协方差 预测 k时刻系统协方差。

1.3 卡尔曼增益方程

卡尔曼增益方程参数：

K_k	卡尔曼增益
R	测量噪声
C	状态变量到测量（观测）的转换矩阵，表示将状态和观测连接起来的关系

方程含义： 计算卡尔曼增益。

R是对象测量噪声的协方差矩阵，它是一个数值，作为已知条件输入滤波器。注意，这个值过大过小都会使滤波效果变差，且R取值越小收敛越快，所以可以通过实验手段寻找合适的R值再利用它进行真实的滤波。

利用极限可以更好理解
预测的方差为0，也就是R变为0，测量不存在误差。我们得到计算出的状态量^x等于测量值，不需要计算，预测值等于观测值，自然收敛速度很快。

另一方面，如果预估误差协方差接近于零，k时刻预估值无方差，且发现卡尔曼增益也为零。因此，这一项对^x(k)的贡献为0。x(k)的计算完全来自于预估值。

1.4 更新最优值方程

更新最优值方程参数：

Y_k	测量值（观测值），是滤波的输入。
^X_k	k时刻的后验估计值

方程含义： 根据k-1时刻的状态预估得到的k时刻状态和观测得到的状态融合得到更为可靠的状态。

根据 状态变量的预测值 和 系统测量值 计算出 k时刻状态变量的最优值。

实际观测和预测观测的残差，和卡尔曼增益一起修正先验（预测），得到后验。

1.5 更新协方差方程

更新协方差方程参数：

Y_k	测量值（观测值），是滤波的输入。
P_k	k 时刻的后验估计协方差

方程含义： P_k和上一个方程生成的^X_k作为迭代值进入下一次迭代。

为了求 k时刻的协方差矩阵。（为得到k+1时刻的卡尔曼输出值做准备）

1.6 卡尔曼滤波总结

卡尔曼滤波器分为两步，预测部分和更新部分。
系统模型用于计算状态预估值和误差协方差P，P是状态预估值的方差且把它当作预测状态中的不确定性的度量。这种不确定性来自过程噪声和预估值 ^x(k- 1)的不确定性的影响。
在算法的最开始，预估值^x(k-1)和P(k- 1)值来自初始估计值。

算法的第二步，使用预测步骤中计算得到的预估值，更新后的状态值及其误差协方差，调整卡尔曼增益K，使更新后的状态值误差协方差最小。

我们假设这个条表示估算值^x(k)的计算。通过调整修正项，卡尔曼增益确定测量值和估计值，对计算 ^x(k)的影响程度。

如果噪声很小，则测量值更可靠，则应对^x(k)的计算贡献更大。

在相反的情况下，预估值的误差很小则预估值更可信。所以^x(k)的计算更多的取决于预估值。

我们更新了方程后，在下一个时间步骤中，修正值用于预测新的预估值，继而重复。要注意的是，要估计当前状态，算法不需要所有过去的信息。只需预估状态值和前一时间步的误差协方差矩阵，以及当前的测量值这就是卡尔曼滤波器递归的原因。

2 Kalman应用——红外测距传感器

使用的是GP2Y0A02YK0F 红外激光测距传感器。

2.1 实物图和接线

接线：

红外激光测距传感器	单片机
红线	+5V
黑线	GND
黄线	输入通道（采样IO）

2.2 测量原理

Sharp 的红外传感器都是基于一个原理， 三角测量原理。 红外发射器按照一定的角度发射红外光束， 当遇到物体以后， 光束会反射回来， 如图所示。 反射回来的红外光线被 CCD 检测器检测到以后， 会获得一个偏移值 L， 利用三角关系， 在知道了发射角度 a， 偏移距 L， 中心矩 X， 以及滤镜的焦距 f 以后， 传感器到物体的距离 D 就可以通过几何关系计算出来了。

可以看到， 当 D 的距离足够近的时候， L 值会相当大， 超过 CCD 的探测范围， 这时， 虽然物体很近， 但是传感器反而看不到了。 当物体距离 D 很大时， L 值就会很小。 这时 CCD 检测器能否分辨得出这个很小的 L 值成为关键， 也就是说 CCD 的分辨率决定能不能获得足够精确的 L 值。

2.3 测量输出

Sharp GS2XX 系列的传感器的输出是非线性的。 没个型号的输出曲线都不同。 所以， 在实际使用前， 最好能对所使用的传感器进行一下校正。 对每个型号的传感器创建一张曲线图， 以便在实际使用中获得真实有效的测量数据。 下图是典型的 Sharp GP2D12 的输出曲线图。

2.4工程配置和代码编写

2.4.1 开启ADC1_IN1

2.4.2 开启串口

2.4.3 代码编写

#include “stdio.h”
int fputc(int c,FILE *stream)
{
	uint8_t ch[1]={c};
	HAL_UART_Transmit(&huart1,ch,1,0xFFFF);
	return c;
}

  while (1)
  {
		float AD_Value;
		HAL_ADC_Start(&hadc1);//启动adc转化
		AD_Value = HAL_ADC_GetValue(&hadc1);
		HAL_Delay(2);
		AD_Value = 3*AD_Value/4095.0;
		printf("ADC_Value=%f\r\n",AD_Value);
		HAL_Delay(100);
  }

//加入均值滤波
		double ADC_Value;
		float Last_ADC_Value;
		HAL_ADC_Start(&hadc1);//启动adc转化
		Last_ADC_Value = HAL_ADC_GetValue(&hadc1);
		HAL_Delay(2);
		for(uint8_t i = 50;i>1;i--)
		{
			ADC_Value = HAL_ADC_GetValue(&hadc1);	
			HAL_Delay(10);
			Last_ADC_Value = ADC_Value + Last_ADC_Value;
		}
		Last_ADC_Value=Last_ADC_Value/50;
		Last_ADC_Value = 3.0*ADC_Value/4095.0;
		printf("ADC_Value=%f\r\n",Last_ADC_Value);
		HAL_Delay(50);

可看到，只要是测量，避免不了产生抖动，因此我们需要很多的电路方法和滤波代码来有效的解决这个问题，使得传感器输出值尽可能平稳。
我们可以看到均值滤波确实起到了一定的作用，但效果不是特别好。
后续会有卡尔曼滤波代码的实现的应用。
————————————————————————————————————————
STM32应用(六)一阶卡尔曼滤波代码和简单应用

2.5 Kalman滤波效果：

我大致将传感器放在据障碍物8cm左右的位置。程色波动的数据是没有添加Kalman滤波的效果，浅蓝色的数据是添加后的效果。