如何用卡尔曼滤波算法求解电池SOC（基础篇）——转载留存

以下是转自：http://www.ind4.net/html/news/newsDetail_6940.html?from=timeline

匈牙利数学家Rudolf Emil Kalman在NASA研究中心访问时发现他提出递推滤波算法有助于提高航天领域对轨道预测的精度。现在在航天、通信、导航、控制、图像处理等计算机技术应用上都少不了对卡尔曼滤波算法（Kalman Filter）的应用。在新能源汽车动力电池的设计中采用卡尔曼滤波算法进行SOC计算也成了比较主流的研究领域。不过我发现目前在网上查阅到的论文、书籍凡是涉及到了这部分内容不是晦涩难懂，就是干脆一笔带过，所以利用周末时间写这篇文章就是想用一种易懂的方式解释卡尔曼滤波算法（以下简称KF算法）。

1. 解决什么问题

如果从哲学的角度理解，我觉得KF算法本质上是解决信任度的问题。我们人类每天获取海量的信息，每时每刻都在思考我该相信什么，该做什么，并在行动后获得新的反馈。计算机也和我们一样，面对相同的困惑。RAM里还保留着上一秒“可疑”的运算的结果，执行器却自信地反馈“他已经有效完成了规定动作”，此时传感器气喘吁吁的跑来报告“我们已经偏离了预定轨道”。这么多答案放在计算机的面前，to be or not to be: that is the question。

解决信任问题有什么好方法么？一般首先想到的会是基于大数据的算法。比如google机器翻译技术就是通过抓取大量网页内容建立庞大的统计学模型从而使得这个产品仅仅开发两年就一举超过众多老牌翻译系统，解决了机器对上下文语境的有效翻译。其实机器并非理解了整段文字，而是根据概率分析出了最优的解释。但是在控制系统上的处理器往往不具备高速联网、大容量存储、大规模计算的能力，同时对计算的时效性和可靠性又有着极高的要求。因此需要采用一种简单高效的算法排除干扰，消除误差。如平均值滤波、中位值滤波、一阶滞后滤波等都是常见的方法。但这些算法运算较为机械，对复杂的控制问题难以得到理想的效果。KF算法则通过持续的递推运算逐渐趋近“真相”，非常适合嵌入式控制系统算法的应用。

2. 含义理解

我看过解释得最通俗易懂的例子是Ramsey Faragher在2012年发表于Signal Processing Magazine上的一篇文章。以小车运动定位为模型，通过几张图片就表达了KF算法的含义。

图1中有一辆停在A点（初始点）的小车准备向前行驶，经过T秒后处于B点。小车上装有速度传感器，因此理论上我们可以根据A点的坐标，计算出B点的坐标。但速度传感器必然存在一定的误差，导致B点坐标并非一个确定的点，而是符合正态分布的坐标可能性。

 

图2中出现了一个小车定位装置，根据定位装置的评估得到了C点坐标。同样定位装置也存在误差，C点坐标的可能性也满足正态分布。此时我们得到了两个小车坐标，应该信任哪一个呢？

 

图3中显示了利用B点和C点两处的正态分布求出了小车实际上更有可能所处的D点坐标，并且D点符合方差（σ）更小的正态分布。同时在后续的计算中D点坐标作为下一次计算中的初始点（A点）。通过不断的迭代向更为准确的坐标逼近。

 

3.公式的初级应用

卡尔曼滤波算法的5项公式如下，其中1式和2式为时间更新方程，意为根据上一次计算的结果推算单位时间后的情况（即(u1, σ1^2)变化为(u2, σ2^2)的过程）。

先验估算值X:   Xp(t) = A*X(t-1) + B*U(t) + W(t)                   

（1）    说明：W(t)为过程噪声

先验协方差P:   Pp(t) = A*P(t-1)*A’+ Q(t)                            

（2）    说明：Q(t)为过程噪声的协方差

3式、4式、5式为测量更新方程，意为根据实际测量情况分别求出信任权重、最优估计值、最新协方差（即求出(u4, σ4^2））。

卡尔曼增益Kg: Kg(t) = Pp(t)*H’/ (H*Pp(t)*H’+ R(t))         

（3）    说明：R(t)为测量噪声的协方差

最优估计值X:   Xo(t) = Xp(t) + Kg(t)(Z(t) - H*Xp(t))              

（4）    说明：Z(t)为实际测量值: Z(t) = H*X(t) + V(t)

最新协方差P:   Po(t) = ( I - Kg(t) *H) * Pp(t)                          

（5）    

 

大多数介绍KF算法的文章难于理解的关键在于没有将公式对应到具体的物理意义上。如果我们拿前面提到的小车运动案例来分析，并将计算值与其中的物理量明确对应起来则明了许多。同时H该案例为单模型（即仅有位置坐标），因此状态转移矩阵A，输入矩阵B都为1。并且测量方式为直接获得位置坐标而无需公式转换，所以观测矩阵H也为1。

 

公式1的物理意义：B点坐标的期望（u2）。根据A点坐标（u1）、位移计算（B*U(t)）、位移计算误差（W(t)）求出。

公式2的物理意义：B点误差的协方差（σ2^2）。根据A点坐标的协方差（σ1^2）、位移计算误差的协方差（Q(t)）求出。   

公式3的物理意义：B点与C点的信任权重，该权重使（σ4^2）最小。根据B点误差的协方差（σ2^2）和实际测量误差的协方差（R(t) ）求出。

公式4的物理意义：A’点坐标的期望（u4）。根据B点坐标的期望（u2）、权重系数（Kg(t)）、实际测量值（u3）求出。

公式4的物理意义：A’点误差的协方差（σ4^2）。根据权重系数（Kg(t)）、B点坐标的协方差（σ2^2）求出。我们可以直观的这么理解，若Kg越大，则说明此次预测值更可信，所以Po越小。

 

接下去我们可以做一道应用题。设小车以匀速10m/s的速度行驶，经过A点（u1=0,σ1^2=1），1秒后到达A’点，此时传感器测得该点位置为11。已知小车位移计算误差的协方差Q(t)为0.5，实际测量误差的协方差R(t) =1。求A’点的最优估计坐标。

首先需要求出B点的信息：

Xp(t) = u2 = u1 + U(t) = 0 + 10 = 10

Pp(t) = σ2^2 = σ1^2 + Q(t) = 1 + 0.5 = 1.5

同时根据实际观测数据我们也知道了C点信息：u3 = 11

然后求信任权重Kg：Kg = σ2^2 / ( σ2^2 + R(t) ) = 1.5 /(1.5 + 1) = 0.6

这样我们就可以得到最优估计A’点坐标期望：Xo(t) = u4 = u2 + Kg（u3 - u2）=10 + 0.6*（11 - 10）=10.6

但为了下一次迭代计算还需要知道A’误差协方差：Po(t) = σ4^2 = （1 - Kg）* σ2^2 = （1 - 0.6）* 1.5 =0.6

由此我们也可以发现A’是比A更“瘦”的正态分布，因此通过持续的迭代，坐标期望u将逐渐趋近真实值。

看到这里读者可能有一个疑问，Q(t)和R(t)通常是怎么得到的？过程噪声协方差Q(t)和测量噪声协方差R(t)往往比较难以获取。在实际应用中可以通过试验进行标定。从便于理解的角度看可以认为若是相信预测结果则Q(t)较小，若相信测量结果则R(t)较小。

 

在实际应用中能把KF算法用好的关键其实就在于标定调参上，如同顶级赛车的性能关键取决于经验丰富的标定工程师的悉心调校。未来汽车工业在硬件上将逐渐走向全球供应链，软件开发上将有越来越成熟丰富的库函数，但即便如此也不代表所有车企都将在同一技术水平线上。一流的性能并非仅仅是软硬件的堆砌，基于海量实验的积累及对算法的深度理解才能给机器注入灵魂。

 

未完待续

 

参考文献：

[1]Ramsey Faragher.Understanding the Basis of the Kalman Filter Via a Simple and Intuitive Derivation.2012