关于线性卡尔曼滤波详细讲解（二）--GPS融合IMU数据

上次我们说了卡尔曼最核心的五个公式，还有一些上次没有说的东西，这次也补全，其实在我们实际需求中，只知道这五个公式，却不知道怎么去根据自己的需求去初始化各种状态转移矩阵之类的，问了一些人很多人会说根据你的模型去建立，然后又陷入了沉思，什么是模型，或者说怎么去建立一个模型，其实这时候个人建议就是根据一个实例去推导卡尔曼的核心公式，深入的理解状态转移矩阵到底是什么，为什么这么定义，然后再根据自己的模型去建立一个矩阵，下面就用matlab去仿真一个例子。

首先我们说我们经常见到的例子也是网上很多都能搜出来的就是什么小车运动，网上也有例子，当你懂了卡尔曼滤波的时候什么状态向量啊，什么状态转移矩阵啊，一眼就知道什么意思了，但是刚接触或者接触不就的肯定会懵掉的，我们先拿一个对室内温度进行卡尔曼滤波的一个实例来分析。

室内温度只有一个状态向量，那就是温度值，根据第一个状态预测公式。
这个右边的X（k|k）就是我上一个时刻算出的最优估计值，Φ就是状态转移矩阵，因为只有一个状态向量，那么我们的状态转移矩阵就是1，当然这个状态预测公式是假设我的房间真实值是恒定的，因为在一段时间内，温度不可能是恒定的，那么就要加入噪声，这个噪声我们可以理解为，由于房间内的空气流通造成了房间真实温度的一些波动，这个也被称为过程噪声Q。这是我们在模拟房间的真实温度值，假设我的真实温度是25.1度，我们再加入一些过程噪声Q，那么真实温度模拟后我们还需要一个模拟出来的温度计测出来的值，怎么模拟呢？由于温度计测量的时候也会有误差的，这个噪声我们叫做测量噪声R，假定温度计测量的值为24.9度，那么由于我们的测量数值也是一维的，那么我们的观测矩阵H = 1，我们可以写出一个由温度计测量的实时数据，我们写作数组形式为：

温度计对k时刻房间温度的测量，卡尔曼滤波是站在温度计角度进行的，它自己也不知道此时的真实状态X（k），只能去利用本次的测量值和上一次估计值来做处理，其目标就是卒子大限度的降低测量噪声R的影响，尽可能的逼近X（k）。那么我们用温度计来做一个matlab例子详细讲解。

N = 120；	%采样点的个数
Xkf = zeros(1,N); %房间各个时刻真实温度值
Z = zeros(1,N);	%观测值，就是温度计测量值
P = zeros(1,N);
%赋初值
X(1) = 25.1； %加入初始值房间温度为25.1
P(1) = 0.01； %初始值的协方差
Z(1) = 24.9； %测量值的初始
Xkf(1) = Z(1);  %初始测量值为24.9度，可以作为滤波器的初始估计状态
%噪声
Q = 0.01;
R = 0.25;
W = sqrt(Q)*randn(1,N); %方差决定噪声的大小
V = sqrt(R)*randn(1,N); %方差决定噪声的大小
%系统矩阵
F = 1;
G = 1;
H = 1;
I = eye(1); %本系统状态为一维
%%%%%%
%以下过程为模拟房间温度和测量值 
for k = 2：N
	X(k) = F*X(k-1) + G*W(k-1);  %真实值 
	Z(k) = H * X(k) + V(k);   %观测值
	%进行卡尔曼滤波
	%两个预测方程，通过上一时刻的滤波值预测下一时刻
	X_pre = F * Xkf(k-1);  %状态预测
	P_pre = F * P(k-1) * F' + Q;  %协方差预测
	%更新方程
	K = P_pre * inv(H * P_pre * H' + R);%计算卡尔曼增益
	e = Z(k) - H * X_pre;	%新息
	Xkf(k) = X_pre + K * e; %状态更新
	P(k) = (1 - K * H) * P_pre; %协方差更新
end
t = 1:N;
plot(t,X,'r',t,Xkf,'g');	

那么仿真结果就是：

可以看到，绿色的滤波值基本和红色的真实值差不了太多，我们可以看看一看误差分析。

可以看到经过卡尔曼滤波之后偏差变小了很多，我们可以调整过程噪声的大小或者测量噪声的大小来调整滤波效果，过程噪声就是在你测量过程中的一些因素，比如摩擦力，风的阻力之类的。而测量噪声就是传感器本身存在的一个噪声误差。

卡尔曼滤波后的值就是在Xkf中，那么对于卡尔曼另一个难点还是在怎么建立模型上，很多时候我们都听过用卡尔曼融合两个数据很好，但是具体怎么融合呢，尤其是刚接触的人会很迷茫，查阅很多资料还是无果，看的更是一头雾水，那么就需要花点功夫在推导上，矩阵的维度是不要错的，举个例子，像是无人机的话，如果我们做一个定高，我们需要一个测量值比较准的传感器和加速度计Z轴的加速度做融合，那我们需要的状态转移矩阵就不是1这么简单了，物理中我们都学过如何通过加速度来计算下一时刻的速度和位移。

首先有状态向量Xkf：
那么由此可以得出状态转换矩阵F为：

那么下面的H我就不多说了，大家推一下就出来了，更加深印象。其实融合传感器数据和加速度融合就是通过加速度数据通过物理的那个公式：X = x0+vt+1/2at^2;来预测，通过实测值和预估值的差再乘上一个增益系数来纠正预估值，使其更接近真实值。