首頁>新聞資訊>行業動態

深入淺出講解卡爾曼濾波(附Matlab程序)

发布时间:2019-07-17 10:28:18 浏览:2103

       简单来说,卡尔曼滤波器是一个“optimal recursive data processing algorithm(最优化自回归数据处理算法)”。对于解决很大部分的问题,他是最优,效率最高甚至是最有用的。他的广泛应用已经超过30年,包括机器人导航,控制,传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理,例如头脸识别,图像分割,图像边缘检测等等。

      在学习卡尔曼滤波器之前,首先看看为什么叫“卡尔曼”。跟其他著名的理论(例如傅立叶变换,泰勒级数等等)一样,卡尔曼也是一个人的名字,而跟他们不同的是,他是个现代人!学过控制的应该都知道,卡尔曼是现代控制理论的奠基人!

      卡尔曼全名Rudolf Emil Kalman,匈牙利数学家,1930年出生于匈牙利首都布达佩斯。1953,1954年于麻省理工学院分别获得电机工程学士及硕士学位。1957年于哥伦比亚大学获得博士学位。我们现在要学习的卡尔曼滤波器,正是源于他的博士论文和1960年发表的论文《A New Approach to Linear Filtering and Prediction Problems》(线性滤波与预测问题的新方法)。

      为了给大家能讲解清楚卡尔曼滤波器,我们找到两篇关于卡尔曼滤波器非常好的文章:

      第一篇来源于CSDN博客,为大家详细的讲解了卡尔曼的原理及应用,算作“深入”

      第二篇来源于知乎,用一个简单的例子,通俗易懂的讲解了卡尔曼滤波,算作“浅出”

       此外,关于卡尔曼滤波的仿真程序在EETOP论坛里有很多,大家可以登录论坛后搜索“卡尔曼”来查找。这里我们给大家提供了一个Matlab仿真程序,可以通过点击左下角的“阅读原文”进入论坛下载。


第一篇


     原文地址:http://blog.csdn.net/lanbing510/article/details/40936343

     1.卡尔曼滤波器的介绍(Introduction to the Kalman Filter)
爲了可以更加容易的理解卡爾曼濾波器,首先應用形象的描述方法來講解,然後我們結合其核心的5條公式進行進一步的說明和探索。結合現代的計算機,其實卡爾曼的程序相當的簡單,只要你理解了他的那5條公式。
在介紹他的5條公式之前,先讓我們來根據下面的例子做個直觀的解釋。

     假设我们要研究的对象是一个房间的温度。根据你的经验判断,这个房间的温度是恒定的,也就是下一分钟的温度等于现在这一分钟的温度(假设我们用一分钟来做时间单位)。假设你对你的经验不是100%的相信,可能会有上下偏差几度。我们把这些偏差看成是高斯白噪声(White Gaussian Noise),也就是这些偏差跟前后时间是没有关系的而且符合高斯分配(Gaussian Distribution)。另外,我们在房间里放一个温度计,但是这个温度计也不准确的,测量值会比实际值偏差。我们也把这些偏差看成是高斯白噪声。

     好了,现在对于某一分钟我们有两个有关于该房间的温度值:你根据经验的预测值(系统的预测值)和温度计的值(测量值)。下面我们要用这两个值结合他们各自的噪声来估算出房间的实际温度值。

     假如我们要估算k时刻的是实际温度值。首先你要根据k-1时刻的温度值,来预测k时刻的温度。因为你相信温度是恒定的,所以你会得到k时刻的温度预测值是跟k-1时刻一样的,假设是23度,同时该值的高斯噪声的偏差是5度(5是这样得到的:如果k-1时刻估算出的最优温度值的偏差是3,你对自己预测的不确定度是4度,他们平方相加再开方,就是5)。然后,你从温度计那里得到了k时刻的温度值,假设是25度,同时该值的偏差是4度。
    由于我们用于估算k时刻的实际温度有两个温度值,分别是23度和25度。究竟实际温度是多少呢?相信自己还是相信温度计呢?究竟相信谁多一点,我们可以用他们的covariance来判断。因为Kg=5^2/(5^2+4^2),所以Kg=0.6098,我们可以估算出k时刻的实际温度值是:23+0.6098*(25-23)=24.22度。可以看出,因为温度计的covariance比较小(比较相信温度计),所以估算出的最优温度值偏向温度计的值。
      现在我们已经得到k时刻的最优温度值了,下一步就是要进入k+1时刻,进行新的最优估算。到现在为止,好像还没看到什么自回归的东西出现。对了,在进入k+1时刻之前,我们还要算出k时刻那个最优值(24.22度)的偏差。算法如下:

      ((1-Kg)*5^2)^0.5=3.12。这里的5就是上面的k时刻你预测的那个23度温度值的偏差,得出的3.12就是进入k+1时刻以后k时刻估算出的最优温度值的偏差(对应于上面的3)。
        就是这样,卡尔曼滤波器就不断的把covariance递归,从而估算出最优的温度值。他运行的很快,而且它只保留了上一时刻的covariance。上面的Kg,就是卡尔曼增益(Kalman Gain)。他可以随不同的时刻而改变他自己的值,是不是很神奇!
      下面就要言归正传,讨论真正工程系统上的卡尔曼。

       2. 卡尔曼滤波器算法(The Kalman Filter Algorithm)

        在这一部分,我们就来描述源于Dr Kalman 的卡尔曼滤波器。下面的描述,会涉及一些基本的概念知识,包括概率(Probability),随即变量(Random Variable),高斯或正态分配(Gaussian Distribution)还有State-space Model等等。但对于卡尔曼滤波器的详细证明,这里不能一一描述。

         首先,我们先要引入一个离散控制过程的系统。该系统可用一个线性随机微分方程(Linear Stochastic Difference equation)来描述,我们结合下面PPT截图进行说明:


      上两式子中,x(k)是k时刻的系统状态,u(k)是k时刻对系统的控制量。A和B是系统参数,对于多模型系统,他们为矩阵。y(k)是k时刻的测量值,H是测量系统的参数,对于多测量系统,H为矩阵。q(k)和r(k)分别表示过程和测量的噪声。他们被假设成高斯白噪声(White Gaussian Noise),他们的covariance分别是Q,R(这里我们假设他们不随系统状态变化而变化)。

      对于满足上面的条件(线性随机微分系统,过程和测量都是高斯白噪声),卡尔曼滤波器是最优的信息处理器。先给出KF算法的流程和五个核心更新方程如下:KF算法




五個更新方程爲:


編寫公式不方便,所以寫成了PDF然後做了截圖粘在了下面,下面就上面的例子和五個核心的公式對Kalman算法進行下說明:

就這樣,算法就可以自回歸的運算下去。

看到這聰明的同學可能已經看出來了,問道卡爾曼增益爲什麽會是第三步中那樣求,現在只大致說一下原理,具體推到比較複雜,有興趣的同學可以參考這文獻去推一推。
還記得前面我們說的誤差協方差矩陣$P_k$麽,即求第k次最優溫度的誤差協方差矩陣,對應于上例中的3和3.12....這些值。看下面PPT,我們最小化P即可得到卡爾曼增益K,對應上例求解K只最小化最優溫度值的偏差,即最小化P(K):

       我们由第四步可以看出,k时刻系统的最优温度值=k-1时刻状态估计值(由上一状态的最优温度值加上过程误差)+带卡尔曼增益权值项的偏差。如果观测误差远远大于估计误差,那么K就很小,k时刻的预测值约等于k时刻的状态估计值,如果对i时刻的状态估计值误差远远大于观测误差,此时相应的q较大,K较大,i时刻的状态估计值更倾向于观察的数据。

卡爾曼濾波器的原理基本描述就完成了,希望能幫助大家理解這這5個公式,其算法可以很容易的用計算機的程序實現。下面,我會用程序舉一個實際運行的例子。

      3.简单例子(A Simple Example)
這裏我們結合第二第三節,舉一個非常簡單的例子來說明卡爾曼濾波器的工作過程。所舉的例子是進一步描述第二節的例子,而且還會配以程序模擬結果。
根第二節的描述,把房間看成一個系統,然後對這個系統建模。當然,我們見的模型不需要非常地精確。我們所知道的這個房間的溫度是跟前一時刻的溫度相同的,所以A=1。沒有控制量,所以u(k)=0。因此得出:
x(k|k-1)=x(k-1|k-1) ……… (6)
式子(2)可以改成:
P(k|k-1)=P(k-1|k-1) +Q ……… (7)
因爲測量的值是溫度計的,跟溫度直接對應,所以H=1。式子3,4,5可以改成以下:
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-X(k|k-1)) ……… (8)
Kg(k)= P(k|k-1) / (P(k|k-1) + R) ……… (9)
P(k|k)=(1-Kg(k))P(k|k-1) ……… (10)


第二篇

考慮軌道上的一個小車,無外力作用,它在時刻t的狀態向量只與相關:
(狀態向量就是描述它的t=0時刻所有狀態的向量,比如:
[速度大小5m/s, 速度方向右, 位置坐标0],反正有了这个向量就可以完全预测t=1时刻小车的状态)

那麽根據t=0時刻的初值,理論上我們可以求出它任意時刻的狀態。
當然,實際情況不會這麽美好。
這個遞推函數可能會受到各種不確定因素的影響(內在的外在的都算,比如刮風下雨地震,小車結構不緊密,輪子不圓等等)導致並不能精確標識小車實際的狀態。
我們假設每個狀態分量受到的不確定因素都服從正態分布。
現在僅對小車的位置進行估計
請看下圖:t=0時小車的位置服從紅色的正態分布。

根據小車的這個位置,我們可以預測出t=1時刻它的位置:


分布變“胖”了,這很好理解——因爲在遞推的過程中又加了一層噪聲,所以不確定度變大了。
爲了避免純估計帶來的偏差,我們在t=1時刻對小車的位置坐標進行一次雷達測量,當然雷達對小車距離的測量也會受到種種因素的影響,于是測量結果告訴我們,小車t=1時的位置服從藍色分布:


好了,現在我們得到兩個不同的結果。前面有人提過加權,Kalman老先生的牛逼之處就在于找到了相應權值,使紅藍分布合並爲下圖這個綠色的正態分布(啰嗦一句,這個綠色分布均值位置在紅藍均值間的比例稱爲Kalman增益(比如下圖中近似0.8),就是各種公式裏的K(t))

你問爲什麽牛逼?
綠色分布不僅保證了在紅藍給定的條件下,小車位于該點的概率最大,而且,而且,它居然還是一個正態分布!
正態分布就意味著,可以把它當做初值繼續往下算了!這是Kalman濾波能夠叠代的關鍵。
最後,把綠色分布當做第一張圖中的紅色分布對t=2時刻進行預測,算法就可以開始循環往複了。
你又要問了,說來說去綠色分布是怎麽得出的呢?
其實可以通過多種方式推導出來,我們課上講過的就有最大似然法、Ricatti方程法,以及上面參考文獻中提及的直接對高斯密度函數變形的方法,這個不展開說了。

關注手機微信

聯系電話

国产精品老司机
88814882 / 88814883

Copyright 2018 国产精品老司机 All Rights Reserved.  陝ICP備12005193號

技術支持/名遠科技