卡尔曼滤波器小白入门

假设有一辆汽车在路上行驶，我们用它的位置和速度来表示当前的状态，写成矩阵的形式，u加速度表示对车的一个控制量，观测左上角的两个公式会发现他们的输出变量是输入变量的线性组合，这就是卡尔曼滤波器是最佳的线性滤波器的原因，因为它只能描述状态和状态之间的线性关系，进一步把两个状态变换矩阵提取出来，公式可以简化为右下角所示，是卡尔曼滤波器的第一个公式：状态预测公式，其中F叫做状态转移矩阵，表示如何从上一时刻的状态来表示当前时刻的状态，B为控制矩阵，表示控制量u如何作用于当前状态，x^表示的是对x的估计量而非真实的值，x的真实状态我们是永远无法知道的，只能根据观测结果尽可能估计x的值，右上角减号的上标表示这个值是根据上一时刻的状态推测而来的。根据观测量修正的x值，才是最佳的估计值可以去掉减号。
从一维的情况分析包含噪声的数据，每次测量值都不同，但都是围绕在一个中心值的周围，表示分布状况简单的方法就是记下中心值和方差。实际上就是假设是一个高斯分布。
如果二维分布的噪声有相关性，需要一个协方差矩阵表示两个维度的额相关程度，对角线的两个值是两个维度的方差，反对角线相等，表示协方差，在卡尔曼滤波器中，所有关于不确定性的表述都要用到协方差矩阵。
噪声协方差矩阵的传递
在小汽车的例子中，每个时刻状态的不确定性都是由协方差矩阵P来表示的。为了使不确定性在每个时刻之间传递，，在P的两边左右各乘一个状态转移矩阵F，

协方差矩阵Q表示预测模型本身带来的噪声，卡尔曼滤波器公式二：这个公式表示不确定想在各个时刻 的传递关系。
假设在公路的一段放置了一个激光测距仪，在每个时刻都可以观测到汽车的位置，观测到的值记为Zt，从汽车本身的状态xt到观测状态Zt之间的变化关系记为H，该变换关系也是线性关系，因为卡尔曼滤波器是线性模型，所以要把H写出矩阵的形式，也就是所谓的观测矩阵。因为观测值也不可靠，因此加入观测噪声v，而这个噪声的协方差矩阵用R来表示。如果还有其他的观测方式，Z就会变成一个多维的列向量会包括每一个测量方式的测量值。而卡尔曼滤波器的融合功能就是在测量矩阵中体现出来的。
状态更新：zt-hxt-表示实际的观测值和预估的观测值的残差，残差乘以系数K就可以修正xt的值了，Kt叫做卡尔曼系数，实际上也是一个矩阵公式如图，卡尔曼系数的作用：1，权衡预测状态协方差P和观察量的协方差R的大小，来决定相信预测模型多一点还是观察模型多一点，如果相信预测模型多一点，残差的权重就会小一点，如果相信观测模型多一点，残差的权重就会大一点。2，把残差的表现形式从观察域转化成状态域。因为观测值Z和状态值x的维度不同，所用的单位甚至描述的特征都是不同的，如何用观测值的残差更新状态值呢？在上述例子里，我们只观测到了汽车的位置，但K里面已经已经包含了协方差矩阵P的信息，利用速度和位置这两个维度的相关性，从位置的残差里面推算出了速度的残差，从而对状态x的两个维度同时进行修正。
更新最佳估计值的噪声分布，是留给下一轮迭代用的，在这步骤里，状态的不确定性减小，在下一轮迭代中，由于传递噪声的引入，不确定性会增大，卡尔曼滤波就是在这种不确定的变化中寻求平衡的。

将卡尔曼滤波器中的五个公式完整列出：

前两个是通过上一时刻的状态来预测当前时刻的状态的，通过这两个公式我们得到的是带减号上标的x,P,这表示这并不是最佳的估计值。另外需要从观测值里面带来的信息。后面的公式是通过当前的观测值来更新x和P，经过更新后的值就是最佳的观测值。
在matlab 里面实现卡尔曼滤波器

Z=(1:100);%观测值，第1秒观测到1米，第2秒观测到2米，匀速直线运动
noise=randn(1,100);%方差为1的高斯噪声
Z=Z+noise;%
X=[0;0];%状态
P=[1 0;0 1];%状态协方差矩阵，给出了XP的初始状态，经过几次迭代之后会接近真实状态
F=[1 1;0 1];%状态转移矩阵，每秒采一次样答塔t是1
Q=[0.0001,0;0 0.0001];%状态转移协方差矩阵
H=[1 0];%观测矩阵
R=1;%观察噪声方差，因为设的是方差为1的高斯噪声，所以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_;%卡尔曼滤波器五个基本公式迭代100次
plot(X(1),X(2));%画点，横轴表示位置，纵轴表示速度
end

从图中可以观察到，从0到100次迭代中只用了很少的几次迭代就已经趋近于真实的状态，纵轴指的是汽车的速度，真实的状态为1，结果保持在1上，证明了卡尔曼滤波器的强大。