卡尔曼滤波

参考链接：https://zhuanlan.zhihu.com/p/34656822

https://www.jianshu.com/p/c512a2b82907

https://blog.csdn.net/CSDN_X_W/article/details/90289021

---------------------------------------------------------------------------------------------

卡尔曼滤波

　　卡尔曼滤波的含义是现时刻的最佳估计为在前一时刻的最佳估计的基础上根据现时刻的观测值作线性修正。

　　卡尔曼滤波（Kalman filtering）一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。

　　卡尔曼滤波是一种递归的估计，即只要获知上一时刻状态的估计值以及当前状态的观测值就可以计算出当前状态的估计

1：数学推导过程

• 被观测的系统

首先我们需要用方程来描述被观测的系统，因为后面滤波器的方程要用到这里的一些参数。卡尔曼滤波器适用于线性系统，设该系统的状态方程和观测方程为：

x(k) = A · x(k-1) + B · u(k) + w(k)
z(k) = H · x(k) + y(k)

x(k) —— k时刻系统的状态
u(k) —— 控制量，无控制输入可简化为0
w(k) —— 符合高斯分布的过程噪声，其协方差在下文中为Q，Q值越大，滤波结果会更偏向于原始数据，噪声越大
z(k) —— k时刻系统的观测值
y(k) —— 符合高斯分布的测量噪声，其协方差在下文中为R，R值越大，曲线越平滑，但滤波器会变得不敏感，会有滞后。
A、B、H —— 系统参数，多输入多输出时为矩阵，单输入单输出时就是几个常数

注意在后面滤波器的方程中我们将不会再直接面对两个噪声w(k)和y(k)，而是用到他们的协方差Q和R。至此，A、B、H、Q、R这几个参数都由被观测的系统本身和测量过程中的噪声确定了。

• 滤波器

　　工程中用到的卡尔曼滤波是一个迭代的过程，每得到一个新的观测值迭代一次，来回来去地更新两个东西：“系统状态”(x)和“误差协方差”(P)。由于每次迭代只用到上一次迭代的结果和新的测量值，这样滤波对计算资源的占用是很小的。

每一次迭代，两个步骤：预测和修正。预测是根据前一时刻迭代的结果，即x(k-1|k-1)【k-1时刻的最优值】和P(k-1|k-1)【k-1时刻（测量噪声）的协方差矩阵】，来预测这一时刻的系统状态和误差协方差，得到x(k|k-1)【用k-1的状态预测k时刻的状态】和P(k|k-1)【k时刻的（测量噪声）协方差矩阵】：

x(k|k-1) = A · x(k-1|k-1) + B · u(k)
P(k|k-1) = A · P(k-1|k-1) · A^T + Q

(这里用到的A、B、H、Q（过程噪声协方差）、R（测量噪声协方差）就是从前面的状态/观测方程中拿来的)
然后计算卡尔曼增益K(k)，和这一次的实际测量结果z(k)一起，用于修正系统状态x(k|k-1)【用k-1的状态预测k时刻的状态】及误差协方差P(k|k-1)【k时刻的（测量噪声）协方差矩阵】，得到最新的x(k|k)【k时刻的最优值】和P(k|k)【k时刻（测量噪声）的协方差矩阵】：

K(k) = P(k|k-1) · H^T · (H · P(k|k-1) · H^T + R)^-1
x(k|k) = x(k|k-1) + K(k) · (z(k) - H · x(k|k-1))
P(k|k) = (I - K(k) · H) · P(k|k-1)

好了好了不会有新的公式了。至此这次迭代就算结束了，x(k|k)就是我们要的滤波后的值，它和P(k|k)将会作为x(k-1|k-1)和P(k-1|k-1)用在下一时刻的迭代里。别看现在公式还是一大堆，在我们所谓的“简单场景”下，系统参数一定下来，就能简化很多很多。

先看状态方程和观测方程。假设我们是在测温度、加速度或者记录蓝牙RSSI，此时控制量是没有的，即：

B · u(k) ≡ 0

另外，参数A和H也简单地取1。现在滤波器的预测方程简化为：

① x(k|k-1) = x(k-1|k-1)
② P(k|k-1) = P(k-1|k-1) + Q

同时修正方程变成：

③ K(k) = P(k|k-1) / (P(k|k-1) + R)
④ x(k|k) = x(k|k-1) + K(k) · (z(k) - x(k|k-1))
⑤ P(k|k) = (1 - K(k)) · P(k|k-1)

①对②、③无影响，于是④可以写成：

x(k|k) = x(k-1|k-1) + K(k) · (z(k) - x(k-1|k-1))

在程序中，该式写起来更简单：

x = x + K * (新观测值 - x);

观察②，对这一时刻的预测值不就是上一时刻的修正值+Q嘛，不妨把它合并到上一次迭代中，即⑤改写成：

P(k+1|k) = (1 - K(k)) · P(k|k-1) + Q

这一时刻的P(k+1|k)，会作为下一时刻的P(k|k-1)，刚好是③需要的。于是整个滤波过程只用这三个式子来迭代即可：

K(k) = P(k|k-1) / (P(k|k-1) + R)
x(k|k) = x(k-1|k-1) + K(k) · (z(k) - x(k-1|k-1))
P(k+1|k) = (1 - K(k)) · P(k|k-1) + Q

• C 程序代码实现

1. 参数列表

2. 代码实现（一维数据滤波）

  实际参数是参照别人已经选好的参数，不过也可以自己改变参数，去观察波形的效果，体会每个参数对于滤波效果的影响，这里不详细介绍。

//1. 结构体类型定义
typedef struct 
{
    float LastP;//上次估算协方差 初始化值为0.02
    float Now_P;//当前估算协方差 初始化值为0
    float out;//卡尔曼滤波器输出 初始化值为0
    float Kg;//卡尔曼增益 初始化值为0
    float Q;//过程噪声协方差 初始化值为0.001
    float R;//观测噪声协方差 初始化值为0.543
}KFP；//Kalman Filter parameter

//2. 以高度为例 定义卡尔曼结构体并初始化参数
KFP KFP_height={0.02,0,0,0,0.001,0.543};

/**
 *卡尔曼滤波器
 *@param KFP *kfp 卡尔曼结构体参数
 *   float input 需要滤波的参数的测量值（即传感器的采集值）
 *@return 滤波后的参数（最优值）
 */
 float kalmanFilter(KFP *kfp,float input)
 {
     //预测协方差方程：k时刻系统估算协方差 = k-1时刻的系统协方差 + 过程噪声协方差
     kfp->Now_P = kfp->LastP + kfp->Q;
     //卡尔曼增益方程：卡尔曼增益 = k时刻系统估算协方差 / （k时刻系统估算协方差 + 观测噪声协方差）
     kfp->Kg = kfp->Now_P / (kfp->NOw_P + kfp->R);
     //更新最优值方程：k时刻状态变量的最优值 = 状态变量的预测值 + 卡尔曼增益 * （测量值 - 状态变量的预测值）
     kfp->out = kfp->out + kfp->Kg * (input -kfp->out);//因为这一次的预测值就是上一次的输出值
     //更新协方差方程: 本次的系统协方差付给 kfp->LastP 威下一次运算准备。
     kfp->LastP = (1-kfp->Kg) * kfp->Now_P;
     return kfp->out；
 }

/**
 *调用卡尔曼滤波器 实践
 */
int height;
int kalman_height=0;
kalman_height = kalmanFilter(&KFP_height,(float)height);

MATLAB 程序代码实现

%卡尔曼滤波实例
%测量房间温度，房间温度真实值为T=25度，一共测量两百个点
N=200;  T=25;  size=[N,1]; 
%取温度预测值的方差为Q=1e-3,温度传感器的测量方差为R=0.36，即我们更相信预测值，而较少相信传感器测量值。
%R值越大，曲线越平滑，但滤波器会变得不敏感，会有滞后。
%Q值越大，滤波结果会更偏向于原始数据，噪声越大
Q=1e-3;  R=0.36;  T_mearsured=T+sqrt(R)*randn(size);
%初始时刻温度的最优估计值为T_start=22.5度，在迭代的过程中，会逐渐收敛。温度初始估计方差为P_start=2
T_start=22.5;  P_start=2;
T_kalman(1)=T_start;  P_kalman(1)=P_start;
%用_kalman的后缀表示最优估计值，用_pre的后缀表示预测值
for k=2:N
    %在进行温度预测时，因为温度是一个连续的状态，我们认为上一时刻的温度和当前时刻的温度相等，则有T(k)=T(k-1)。u(k)=0
    T_pre(k)=T_kalman(k-1); % 估计时刻k 的状态
    P_pre(k)=P_kalman(k-1)+Q; % 计算误差相关矩阵P, 度量估计值的精确程度
    K(k)=P_pre(k)/(P_pre(k)+R); % 计算卡尔曼增益
    T_kalman(k)=T_pre(k)+K(k)*(T_mearsured(k)-T_pre(k)); % 更新状态变量
    P_kalman(k)=P_pre(k)-K(k)*P_pre(k); % 更新误差相关矩阵P 
end
%画图
figure();
plot(T*ones(size),'g');
hold on
plot(T_mearsured,'b');
hold on
plot(T_kalman,'r');
legend('温度真实值','温度测量值','Kalman估计值')