卡尔曼滤波简单理解及C语言代码

因为毕业设计做室内定位，需要对RSSI值进滤波，采用卡尔曼滤波。网上看了很多卡尔曼滤波的文章，为了加深自己的印象。所以打算结合网上的资料加上自己理解方式写一篇文章（如果不对还请您帮忙纠正）。
在理解卡尔曼之前我们先讲个故事，比如现在我们要测一个房间的温度，那么我们需要一个温度计。假设市场上买两种温度计，一种测得温度非常准没有误差。另一种只能大概测量。当然测得准的温度计，比较贵，另外一个比较便宜。如果你是土豪那么你可以买比较贵的就是没有误差的，那么你就跟卡尔曼没有故事了。
现在假设你是一个中等屌丝，你可以卖得起两个破的温度计，现在两个温度计测量得到两个值，我们假设他一个是23度，一个是25度，那么实际应该是多少度呢。如果你对他们求平均（即权值是0.5,0.5），你会得到24度左右。再说的明确一点呢24±1度，应为这个是你猜的，所以你不能肯定，那么这个1度就是你的不确定度。可能你是天蝎座的，比较偏心，不想然两个温度计都雨露均沾，我们假设你比较相信前一个温度计，不相信另一个，这里我们取权值为（0.7，0.3），那么你得到的温度值就是0.723+0.325=23.6度，那么不确定度可能是1（感觉自己偏心的有道理，猜的比较准），可能是6度（这个6是随便写的，猜的不准误差比较大）。根据上面这个问题我们发现
1.不确定我们猜的准不准，换句话来说就是不确定度不知道多少
2.权值该怎么取才合理

下面我们来看一下卡尔曼先生是怎么做的。还是原来的例子。假设现在你是一个终极大屌丝，只买的起一个温度计了。但是你有一个特异功能，猜的很准就是你睁开眼就知道今天是几度，该穿大裤衩还是长袖（相当于你有一个温度模型）。之前讲的不确定度，这个太low了，我们换一种高级的说法，卡尔曼先生用协方差来替代不确定度。我们现在再来一个假设。

假设有21:19分房间的温度是23度，协方差是9（原来是讲不确定度）。
好，时间很快现在21:20分了，这一分钟时间内没有阳光直射，没有开着空调，所以你发挥你的神力，猜测现在温度和之前差不多也是23度（也就是现在的温度等于之前的温度），协方差是16（你对你神力的信任值，随便写的）。
现在我们有一个相同的温度值，但是有两个协方差（9和16）。现在把两个协方差整合在一起。这样可能会更准一点。该怎么整合呢，卡尔曼先生告诉我们直接加起来就行了。通过神力及卡尔曼先生的方法，我们得到了23度，协方差25。
这个结果基本是靠猜的，我们称他为预测阶段。

接下来别忘了我们还有一个温度计，从温度计上，读出的温度值是25度，协方差是16（随便写的）。现在又有两个值了。
1. 23度，协方差25（预测阶段得到了）
2. 25度，协方差16（温度计读出）
我们该如何加权呢？（0.5，0.5）又或者是（0.3,0.7）。卡尔曼先生告诉我们可以用两个协方差得到权值**（25/(25+16)）^0.5=0.78**,通常又把这个结果称为卡尔曼增益。有了这个结果就可以计算出21:20分的温度值了，0.78*25+（1-0.78）23=24.56度，那么21:20分的协方差是多少呢，卡尔曼先生同样给了一个公式*（1-0.78）25（预测协方差）=5.5*
有了21:20分的温度值，及协方差，我们就可以重复上面的步骤，进行迭代。
对上面过程的特殊数据，一般化。
1.假设21：19分温度为23及协方差为9（分别用X(k-1|k-1)和P(k-1|k-1)来表示）
2.猜测21:20分温度与21：19分温度相同，协方差为16（分别用X(k|k-1)和Q来表示）
3.计算温度，及协方差（结果用P(k|k-1)表示）可以得到下面两个公式
X(k|k-1)=X(k-1|k-1)；
P(k|k-1)=P(k-1|k-1)+Q;
4. 读温度计的值为25，猜测协方差为16（分别用Z(k)和R表示）
5.计算权值，即卡尔曼增益（用K(k)来表示）
6.计算21:20分温度及协方差（分别用X(k|k)和P(k|k)表示）可以得到下面三个公式
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)
这五个公式就是，对卡尔曼五个核心公式进行简化，有了这五个公式可以很方便的在单片机上实现

float kalmanFilter_A(float inData) 
{
  static float prevData=0; 
  static float p=10, q=0.0001, r=0.005, kGain=0;
    p = p+q; 
    kGain = p/(p+r);

    inData = prevData+(kGain*(inData-prevData)); 
    p = (1-kGain)*p;

    prevData = inData;

    return inData; 
}

其中p的初值可以随便取，但是不能为0（为0的话卡尔曼滤波器就认为已经是最优滤波器了）
q,r的值需要我们试出来，讲白了就是(买的破温度计有多破，以及你的超人力有多强)

r参数调整滤波后的曲线与实测曲线的相近程度，r越小越接近。

q参数调滤波后的曲线平滑程度，q越小越平滑。
具体过程可以以看一下这篇博客