卡尔曼滤波器的原理以及在matlab中的实现- 学习笔记
卡尔曼滤波器又叫最佳线性滤波器，好处有实现简单，纯时域（无须频域变换）所以在工程上有很广泛的应用。
假设有一辆小车在路上行驶，用它的位置和速度表示它当前的状态，写成矩阵形式就是一个二维的列向量，这个列向量的两个元素就是它的两个状态，u表示驾驶员对车辆的控制，即加速度，如果匀速运动，u就是0.如果我们知道上一个时刻的状态，Xt-1，则当前时刻的位置状态信息和速度状态信息为：
上式中Pt-1 为上一时刻的速度，Vt-1 为上一时刻的速度，deltet为两个时刻的间隔，Ut为加速度
这两个变量中的输出变量都是输入变量的线性组合，这就是为什么说 卡尔曼滤波器是最佳的线性滤波器，因为它只能描述状态与状态之间的线性关系。既然是线性关系，我们就可以进一步写成矩阵的形式：
我们进一步把两个状态变换矩阵提取出来，变成由此可得：
，这个公式就是卡尔曼滤波中的第一个公式，状态预测公式，其中F叫做状态转移矩阵，表示我们如何从上一刻的状态来推测当前时刻的状态。
B叫做控制矩阵，表示控制量u如何作用于当前状态。x上的尖号表示这是对x的估计量，而不是x的真实值，因为汽车的真实状态我们无法知道，我们只能根据观测尽可能的估计x的值。x上的减号上标表示这个值是根据上一时刻推测出来的，需要修正，修正之后的值才是最佳的估计值，也就是没有减号上标的x。
有了这个公式我们就可以推测下一个时刻的状态，所有的推测都包含噪声，我们可以用协方差矩阵来表示噪声。
什么是协方差矩阵呢？
假设我们有一个一维的包含噪声的数据，每次测量的值都不同，但都是围绕在一个中心的周围，最简单的方法就是记录下它的中心值和方差， 这实际上是假设它是一个高斯分布。
如果是二维的情况，那么它包含噪声的数据看起来是如下所示， 分别对两个坐标轴进行投影，在两个轴上都是高斯分布。如果两个维度是独立的，可以分别记录其中心值和方差如下图1，但如果两个维度有相关性， 比如在一个维度上噪声增大的时候，另一个维度上噪声也增大，图形就会变成下图2所示：
如果一个维度增大，另一个维度减小，就会变成下图：
所以要表示两个维度之间的相关性，除了要有一个方差之外，还要有一个协方差表示两个维度之间的相关程度，写成矩阵的形式如下图所示：
对角线上的两个值是两个维度的方差，反对角线上的两个值是相等的，是他们的协方差，上面3个例子的协方差分别是：
，在卡尔曼滤波器中，所有关于不确定性的表述都要用到协方差矩阵。在小汽车的例子中，在我们小汽车的例子中，每一个时刻的状态的不确定性都是由协方差矩阵P来表示，下一个问题是，如何让这种不确定性在每个时刻之间传递呢？答案就是乘上状态转换矩阵F，
,写得具体一点就是
，从上一时刻推测当前时刻的协方差，就等于上一时刻的协方差两边乘以状态转移矩阵， 至于为什么要乘以两边，这是由协方差矩阵的性质决定的：
，由于预测模型本身不是百分百准确的，本身也是包含噪声的，所以我们也要在后面加上协方差矩阵Q来表示预测模型本身带来的噪声，
，这个公式是卡尔曼滤波器的第二个公式，表示不确定性在各个时刻之间的传递关系。
回到小汽车的例子，假设我们在小汽车的一端放置一个激光测距仪，可以观察到每一时刻小汽车的位置，，那么从汽车本身的状态Xt 到 观测状态Zt之间有一个变换关系，我么们记为H， 这个变换关系也是线性关系，因此我们可以把H写成矩阵的形式，也就是观测矩阵，
，在这个例子里，X是一个二维的列向量，Z只是一个标量的值，所以H是一个一行两列的矩阵，里面的元素分别是1，0，这样H和X相乘的时候就可以得到标量的值 Z，
，Z也就是汽车的位置，它和X的第一个元素是相等的，后面加一个小写v的原因是我们的观测值也不是百分之百可靠的，v表示观测的噪声。
，在这个例子里，观测值是一个一维的值，因此R的形式也不是一个一维的矩阵，而是一个单独的值，表示Z的方差。如果我们除了激光测距仪还有别的方法可以观测到汽车的某项特征，那么Z就是一个多维的列向量， 会包含每一种测量方式的测量值。而每一个测量值都只是真实状态的一种不完全表现。可以从几种不完全的表述中推断出真实的状态。
我们前面已经得到了带尖号的Xt,现在我们只需要在它后面再加上一项，来修正它的值，就可以得到最佳估计值了
，
加上的这项，先看括号里面的部分，，表示实际的观测值和我们预期的观测值之间的残差，残差乘以系数K就可以修正Xt的值了，这个K叫做卡尔曼系数，实际上它也是一个矩阵，公式是这样的：
，卡尔曼系数K作用主要是两方面，一是权衡预测状态协方差P和观察量的协方差矩阵R的大小，来决定我们是相信预测模型多一点还是观察模型多一点，
如果相信预测模型多一点，那么这个残差的权重就会小一点，如果相信观察的模型小一点，这个残差的权重就会大一点。第二个作用就是将残差的表现形式从观察域转换到状态域，刚才讲到观察值Z只是一个一维的向量，状态X是一个二维的向量，它们所用的单位甚至描述的特征都有可能是不同的，那如何用观察值的残差取更新状态值呢？卡尔曼系数K就是在替我们做这样的转换。在我们这个例子里，我们只观察到了汽车的位置，但K里面已经包含了协方差矩阵K的信息，所以它利用位置和速度这两个维度的相关性，从位置的残差里面推测出了速度的残差，从而可以让我们对状态X的两个维度同时进行修正。
我们现在只差最后一步了，就是更新最佳估计值的噪声分布，这个值是留给下一轮迭代用的。
在这一步里，状态的不确定性是减小的，而在下一轮里，由于传递噪声的引入，不确定性又会增大，卡尔曼滤波器就是在这种不确定性的变化中寻求一种平衡。
到现在为止，我们已经有了卡尔曼滤波器所有的5个公式：
前两个是通过上一时刻的状态来预测当前时刻的状态的，通过这两个公式，我们得到带减号上标的x和p，这表示这并不是最佳的估计值，减号上标表示还欠缺点什么东西，欠缺的东西就是从观测值哪里带来的信息。因为我们还没有考虑当前时刻的观测值。后面的三个公式就是用当前的观测值更新x和p，经过更新后的值，就是最佳的观测值了，所以他们是不带尖号上标的。
如何在matlab里面实现卡尔曼滤波器：
示例代码：
Z=(1:100); %小汽车观测值
noise=randn(1,100); %方差为1的高斯噪声
Z=Z+noise;
X=[0; 0]; %状态
P=[1 0; 0 1]; %状态协方差矩阵
F=[1 1; 0 1]; %状态转移矩阵
Q=[0.0001, 0; 0 0.0001]; %状态转移协方差矩阵
H=[1 0]; %观测矩阵
R=1; %观测噪声方差
figure;
hold on;
for i=1:100
X_ = F*X;
P_ = F*P*F'+Q;
K = P_*H'/(H*P_*H'+R);
X = X_+K*(Z(i)-H*X_);
P = (eye(2)-K*H)*P_;
plot(X(1), X(2)); %画点，横轴表示位置，纵轴表示速度
end
最终结果
纵轴表示汽车速度，真实状态为1，所以我们看到它一直保持在1这个水平上。