當(dāng)前位置：首頁 > 人文社科 > 生活经验 >内容正文

生活经验

我所理解的卡尔曼滤波——公式推导与应用

發(fā)布時間：2023/11/27 生活经验 76 豆豆

生活随笔收集整理的這篇文章主要介紹了我所理解的卡尔曼滤波——公式推导与应用小編覺得挺不錯的,現(xiàn)在分享給大家,幫大家做個參考.

我所理解的卡爾曼濾波——公式推導(dǎo)與應(yīng)用

1.什么是卡爾曼濾波
2.卡爾曼濾波的數(shù)學(xué)推導(dǎo)
- 2.1 狀態(tài)方程和測量方程
- 2.2 卡爾曼濾波過程
3 卡爾曼濾波應(yīng)用

1.什么是卡爾曼濾波

先舉個例子說一下什么是卡爾曼濾波。

假如有一個小機器人可以在草地上自由的運動。為了讓它實現(xiàn)導(dǎo)航，機器人需要知道自己所處的位置。那么這個小機器人如何才能知道自己在什么位置呢？有兩種方式：

根據(jù)起始位置及自身的運動進行運動學(xué)計算，從而得到自身的位置信息。
根據(jù)自身攜帶的傳感器測量自身的位置信息，如GPS。

那么，現(xiàn)在就存在一個問題。不論是根據(jù)運動學(xué)計算還是利用傳感器信息，得到的位置信息都不可避免的存在誤差。那機器人到底該相信哪個數(shù)據(jù)呢？有或者，如何根據(jù)這兩種數(shù)據(jù)來確定自身的位置呢？最簡單的方式就是取平均值，但是這種方式合理嗎？憑什么每一項的權(quán)重都是0.5？顯然這種方式不合適。

既然，取均值的方式不合理，那又該怎么做呢？卡爾曼濾波解決的就是這個問題！

卡爾曼濾波的本質(zhì)就是根據(jù)”測量值”（如GPS數(shù)據(jù)）和 “預(yù)測量”（如運動學(xué)計算結(jié)果）及 “誤差”，計算得到當(dāng)前狀態(tài)的最優(yōu)量。

2.卡爾曼濾波的數(shù)學(xué)推導(dǎo)

2.1 狀態(tài)方程和測量方程

首先假設(shè)我們知道一個線性系統(tǒng)的狀態(tài)差分方程為 $(1)xk=Axk?1+Buk?1+wk?1x_k = Ax_{k-1}+Bu_{k-1}+w_{k-1} \tag{1}$ 其中 $x$ 是系統(tǒng)的狀態(tài)向量，大小為n*1列。 $A$ 為轉(zhuǎn)換矩陣，大小為 $n ? n$ 。 $u$ 為系統(tǒng)輸入，大小為 $k ? 1$ 。 $B$ 是將輸入轉(zhuǎn)換為狀態(tài)的矩陣，大小為 $n ? k$ 。隨機變量 $w$ 為系統(tǒng)噪聲。

這里說句題外話，什么是狀態(tài)方程？說的通俗一點，就是預(yù)測方程。根據(jù)上一時刻的狀態(tài)及控制量來預(yù)測當(dāng)前時刻的狀態(tài)，當(dāng)然，這個過程中不可避免的存在誤差。

系統(tǒng)的測量方程為 $(2)zk=Hxk+vkz_k=Hx_k+v_k\tag{2}$ $z$ 是測量值，大小為 $m ? 1$ ， $H$ 也是狀態(tài)變量到測量的轉(zhuǎn)換矩陣。大小為 $m ? n$ 。隨機變量 $v$ 是測量噪聲。

那什么又是測量方程呢？就是根據(jù)此刻的狀態(tài)量及噪聲得到的測量值的過程。另一方面測量方程也表明，測量值是由我們的狀態(tài)值確定的。

對于狀態(tài)方程中的系統(tǒng)噪聲 $w$ 和測量噪聲 $v$ ，假設(shè)服從如下多元高斯分布，并且 $w$ , $v$ 是相互獨立的。其中 $Q$ , $R$ 為噪聲變量的協(xié)方差矩陣。

2.2 卡爾曼濾波過程

在此，我們設(shè) $xk^′\hat{x_k}'$ 為預(yù)測值（根據(jù)上一時刻的狀態(tài)及控制量由狀態(tài)方程得到的當(dāng)前時刻的狀態(tài)）、 $xk^\hat{x_k}$ 為估計值（即卡爾曼濾波的最終的結(jié)果）、 $zk^\hat{z_k}$ 是測量值的預(yù)測（即，根據(jù)預(yù)測的狀態(tài)值 $xk^′\hat{x_k}'$ 得到的測量值）。則三者的關(guān)系為 $(3)xk^=xk^′+K(zk?zk^)=xk^′+K(zk?Hxk^′)\hat{x_k} = \hat{x_k}'+K(z_k-\hat{z_k})=\hat{x_k}'+K(z_k-H\hat{x_k}') \tag{3}$

其中， $(zk?Hxk^′)(z_k-H\hat{x_k}')$ 稱之為殘差，也就是預(yù)測的和你實際測量值之間的差距。如果這項等于0，說明預(yù)測和測量出的完全吻合。

這個公式說明什么呢？他的意思就是最終的估計值等于預(yù)測值加上乘以系數(shù) $K$ 的測量殘差。

從這個公式可以看出，我們可以根據(jù)狀態(tài)方程得到預(yù)測值，根據(jù)測量方程可以得到測量值的預(yù)測，然后在根據(jù)公式3就可以得到最終的估計值。還有一個問題就是系數(shù) $K$ 的確定。

××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××

接下來說一下參數(shù) $K$ 的確定。

首先來看一下真實值與估計值之間協(xié)方差
$(4)PK=E(ekekT)=E[(xk?xk^)(xk?xk^)T]P_K = E(e_ke_k^T) = E[(x_k-\hat{x_k})(x_k-\hat{x_k})^T] \tag{4}$
將公式3中的估計值帶入可得
$(5)PK=E[(xk?xk^′?K(zk?zk^))(xk?xk^′?K(zk?zk^))T]=E[(xk?xk^′?K(Hxk+vk?Hxk^′))(xk?xk^′?K(Hxk+vk?Hxk^′))T]=E[((I?KH)xk?xk^′?Kvk+KHxk^′)((I?KH)xk?xk^′?Kvk+KHxk^′)T]=E[((I?KH)(xk?xk^′)?Kvk)((I?KH)(xk?xk^′)?Kvk)T]P_K = E[(x_k-\hat{x_k}'-K(z_k-\hat{z_k}))(x_k-\hat{x_k}'-K(z_k-\hat{z_k}))^T] \\ = E[(x_k-\hat{x_k}'-K(Hx_k+v_k-H\hat{x_k}'))(x_k-\hat{x_k}'-K(Hx_k+v_k-H\hat{x_k}'))^T] \\ = E[((I-KH)x_k-\hat{x_k}'-Kv_k+KH\hat{x_k}')((I-KH)x_k-\hat{x_k}'-Kv_k+KH\hat{x_k}')^T] \\ = E[((I-KH)(x_k-\hat{x_k}')-Kv_k)((I-KH)(x_k-\hat{x_k}')-Kv_k)^T] \tag{5}$

同理，預(yù)測值與真實值之間的寫方差矩陣為
$(6)PK′=E[ek′ek′T]=E[(xk?xk^′)(xk?xk^′)T]=E[(A(xk?1?xk?1^)+wk?1)(A(xk?1?xk?1^)+wk?1)T]=E[(Aek?1)(Aek?1)T]+E[wk?1wk?1T]=APk?1AT+QP_K' = E[e_k'e_k'^T] = E[(x_k-\hat{x_k}')(x_k-\hat{x_k}')^T] \\=E[(A(x_{k-1}-\hat{x_{k-1}})+w_{k-1})(A(x_{k-1}-\hat{x_{k-1}})+w_{k-1})^T] \\=E[(Ae_{k-1})(Ae_{k-1})^T] +E[w_{k-1}w_{k-1}^T] = AP_{k-1}A^T+Q\tag{6}$
系統(tǒng)狀態(tài)x變量和測量噪聲之間是相互獨立的。所以，公式5可以寫為
$(7)PK=E[((I?KH)(xk?xk^′)?Kvk)((I?KH)(xk?xk^′)?Kvk)T]=(I?KH)E[(xk?xk^′)(xk?xk^′)T](I?KH)T+KE[vkvkT]KTP_K = E[((I-KH)(x_k-\hat{x_k}')-Kv_k)((I-KH)(x_k-\hat{x_k}')-Kv_k)^T] \\ =(I-KH)E[(x_k-\hat{x_k}')(x_k-\hat{x_k}')^T](I-KH)^T+KE[v_kv_k^T]K^T \tag{7}$

將6式帶入7式可得
$(8)PK=(I?KH)PK′(I?KH)T+KE[vkvkT]KT=PK′?KHPk′?PK′HTKT+K(HPk′HT+R)KTP_K =(I-KH)P_K'(I-KH)^T+KE[v_kv_k^T]K^T = P_K'-KHP_k'-P_K'H^TK^T +K(HP_k'H^T+R)K^T\tag{8}$

協(xié)方差矩陣的對角線元素就是方差。這樣一來，把矩陣 $P ? K$ 的對角線元素求和，用字母T來表示這種算子，他的學(xué)名叫矩陣的跡。則有
$(9)T[PK]=T[PK′]?T[KHPk′]?T[PK′HTKT]+T[K(HPk′HT+R)KT]T[P_K] = T[P_K']-T[KHP_k']-T[P_K'H^TK^T] +T[K(HP_k'H^T+R)K^T]\tag{9}$

這樣，我們就得到了關(guān)于K的一個方程。對其求導(dǎo)可得：

令導(dǎo)數(shù)為0，可以求得：
$=P_k'H^T (HP_k'H^T+R)^{-1}\tag{10}$

將K值帶入公式8中，可得：
$(11)PK=(I?KH)Pk′P_K = (I-KH)P_k'\tag{11}$
×××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××

至此，推到過程結(jié)束。我們來理一下卡爾曼濾波的思路：

1.首先，根據(jù)公式1計算我們的預(yù)測值，根據(jù)公式計算預(yù)測值與真實值的協(xié)方差矩陣 $xk^′=Ax^k?1+Buk?1\hat{x_k}' = A\hat{x}_{k-1}+Bu_{k-1}$ $PK′=APk?1AT+QP_K' = AP_{k-1}A^T+Q$
2.根據(jù)1中得到的數(shù)據(jù)計算卡爾曼增益 $K$ $=P_k'H^T (HP_k'H^T+R)^{-1}$
3.根據(jù)預(yù)測值和測量值以及卡爾曼增益來計算最終的結(jié)果 $xk^=xk^′+K(zk?Hxk^′)\hat{x_k} =\hat{x_k}'+K(z_k-H\hat{x_k}')$
4.最后還要計算估計值和真實值之間的誤差協(xié)方差矩陣，為下次遞推做準(zhǔn)備。 $PK=(I?KH)Pk′P_K = (I-KH)P_k'$

卡爾曼濾波過程就是不斷利用以上5個公式進行計算的過程。

3 卡爾曼濾波應(yīng)用

此部分內(nèi)容來自：https://blog.csdn.net/m0_38089090/article/details/79523784

應(yīng)用背景是勻加速小車，該線性系統(tǒng)的狀態(tài)差分方程為 $x_k = Ax_{k-1}+Bu_{k-1}+w_{k-1}$
對小車進行建模，ft為合力，小車的狀態(tài)方程表示為

矩陣形式表示為

#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <eigen3/Eigen/Dense>//包含Eigen矩陣運算庫，用于矩陣計算
#include <cmath>
#include <limits>//用于生成隨機分布數(shù)列using namespace std;
using Eigen::MatrixXd;double generateGaussianNoise(double mu, double sigma);//隨機高斯分布數(shù)列生成器函數(shù)int main(int argc, char* argv[])
{//""中是txt文件路徑，注意：路徑要用//隔開ofstream fout("..//result.txt");const double delta_t = 0.1;//控制周期，100msconst int num = 100;//迭代次數(shù)const double acc = 10;//加速度，ft/mMatrixXd A(2,2);A(0,0) = 1;A(1,0) = 0;A(0,1) = delta_t;A(1,1) = 1;MatrixXd B(2,1);B(0,0) = pow(delta_t,2)/2;B(1,0) = delta_t;MatrixXd H(1,2);//測量的是小車的位移，速度為0H(0,0) = 1;H(0,1) = 0;MatrixXd Q(2,2);//過程激勵噪聲協(xié)方差，假設(shè)系統(tǒng)的噪聲向量只存在速度分量上，且速度噪聲的方差是一個常量0.01，位移分量上的系統(tǒng)噪聲為0Q(0,0) = 0;Q(1,0) = 0;Q(0,1) = 0;Q(1,1) = 0.01;MatrixXd R(1,1);//觀測噪聲協(xié)方差，測量值只有位移，它的協(xié)方差矩陣大小是1*1，就是測量噪聲的方差本身。R(0,0) = 10;//time初始化，產(chǎn)生時間序列vector<double> time(100, 0);for(decltype(time.size()) i = 0; i != num; ++i){time[i] = i * delta_t;//cout<<time[i]<<endl;}MatrixXd X_real(2,1);vector<MatrixXd> x_real, rand;//生成高斯分布的隨機數(shù)for(int i = 0; i<100;++i){MatrixXd a(1,1);a(0,0) = generateGaussianNoise(0,sqrt(10));rand.push_back(a);}//生成真實的位移值for(int i = 0; i < num; ++i){X_real(0,0) = 0.5 * acc * pow(time[i],2);X_real(1,0) = 0;x_real.push_back(X_real);}//變量定義，包括狀態(tài)預(yù)測值，狀態(tài)估計值，測量值，預(yù)測狀態(tài)與真實狀態(tài)的協(xié)方差矩陣，估計狀態(tài)和真實狀態(tài)的協(xié)方差矩陣，初始值均為零MatrixXd X_evlt = MatrixXd::Constant(2,1,0), X_pdct = MatrixXd::Constant(2,1,0), Z_meas = MatrixXd::Constant(1,1,0),Pk = MatrixXd::Constant(2,2,0), Pk_p = MatrixXd::Constant(2,2,0), K = MatrixXd::Constant(2,1,0);vector<MatrixXd> x_evlt, x_pdct, z_meas, pk, pk_p, k;x_evlt.push_back(X_evlt);x_pdct.push_back(X_pdct);z_meas.push_back(Z_meas);pk.push_back(Pk);pk_p.push_back(Pk_p);k.push_back(K);//開始迭代for(int i = 1; i < num; ++i){//預(yù)測值X_pdct = A * x_evlt[i-1] + B * acc;x_pdct.push_back(X_pdct);//預(yù)測狀態(tài)與真實狀態(tài)的協(xié)方差矩陣，Pk'Pk_p = A * pk[i-1] * A.transpose() + Q;pk_p.push_back(Pk_p);//K:2x1MatrixXd tmp(1,1);tmp = H * pk_p[i] * H.transpose() + R;K = pk_p[i] * H.transpose() * tmp.inverse();k.push_back(K);//測量值zZ_meas = H * x_real[i] + rand[i];z_meas.push_back(Z_meas);//估計值X_evlt = x_pdct[i] + k[i] * (z_meas[i] - H * x_pdct[i]);x_evlt.push_back(X_evlt);//估計狀態(tài)和真實狀態(tài)的協(xié)方差矩陣，PkPk = (MatrixXd::Identity(2,2) - k[i] * H) * pk_p[i];pk.push_back(Pk);}cout<<"含噪聲測量"<<"  "<<"后驗估計"<<"  "<<"真值"<<"  "<<endl;for(int i = 0; i < num; ++i){cout<<z_meas[i]<<"  "<<x_evlt[i](0,0)<<"  "<<x_real[i](0,0)<<endl;fout<<z_meas[i]<<"  "<<x_evlt[i](0,0)<<"  "<<x_real[i](0,0)<<endl;//輸出到txt文檔，用于matlab繪圖//cout<<k[i](1,0)<<endl;//fout<<rand[i](0,0)<<endl;//fout<<x_pdct[i](0,0)<<endl;}fout.close();return 0;
}//生成高斯分布隨機數(shù)的函數(shù)，網(wǎng)上找的
double generateGaussianNoise(double mu, double sigma)
{const double epsilon = std::numeric_limits<double>::min();const double two_pi = 2.0*3.14159265358979323846;static double z0, z1;static bool generate;generate = !generate;if (!generate)return z1 * sigma + mu;double u1, u2;do{u1 = rand() * (1.0 / RAND_MAX);u2 = rand() * (1.0 / RAND_MAX);}while ( u1 <= epsilon );z0 = sqrt(-2.0 * log(u1)) * cos(two_pi * u2);z1 = sqrt(-2.0 * log(u1)) * sin(two_pi * u2);return z0 * sigma + mu;
}

測試結(jié)果為：

總結(jié)

以上是生活随笔為你收集整理的我所理解的卡尔曼滤波——公式推导与应用的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。

上一篇：城管随意贴罚单怎么处理？
下一篇：刷题：二叉树的遍历方式及根据遍历结果还原