卡爾曼濾波器學習筆記（上）

最近在學習probablistic robotics這本書，獲益良多。以前學了概率論和隨機過程之後一直覺得這些是虛的，不知道在工程上怎麼用，而這本書恰恰就是講如何把這些概率理論和方差估計應用到工程上去，更確切的說，應用到機械人上去。

要應用kalman filter,首先要有三個前提假設：

kalman filter五條**公式：

這五條公式基本上就是kalman filter的主要內容了，它的本質就是通過**結合測量來估計當前系統的狀態。舉個lizi，假如我們要估計一架飛行器的姿態，可以通過imu來實時測量，但是測量值有一定的風險是不準確的，所以並不能完全依賴感測器。任何乙個滿足物理規律的系統應當是連續的，所以我們還可以通過上一狀態來**當前狀態。kalman filter正是結合這兩條進行狀態估計，到底是相信哪乙個多一點，還要根據kt來決定，我們定義kt為卡爾曼增益，它是根據測量和**的協方差來計算的。

下面逐條解釋：

現在需要利用kalman filter來估計房間的實時溫度

% 初始化引數
n_iter = 100;       %計算連續n_iter個時刻
sz = [n_iter, 1]; 
x = 24;             % 溫度的真實值
q = 4e-4;           % 對溫度**值的方差
r = 0.25;           % 測量方差，反應溫度計的測量精度
t_start = 23.5;     %溫度初始估計值
delta_start = 1;    %溫度初始估計方差
z = x + sqrt(r)*randn(sz); 
% z是溫度計的測量結果，在真實值的基礎上加上了方差為0.25的高斯雜訊。
% 初始化陣列
state_kalman=zeros(sz); 
% 對溫度的估計值。即在k時刻，結合溫度計當前測量值與k-1時刻**值，得到的最終估計值
variance_kalman=zeros(sz);         % 估計值的方差
state_pre=zeros(sz); % 對溫度的**
variance_pre=zeros(sz);    % **值的方差
k=zeros(sz);         % 卡爾曼增益
state_kalman(1) = t_start;   %溫度估計值初始化
variance_kalman(1) =delta_start;   %估計值方差初始化
%%開始迭代計算
for k = 2:n_iter
state_pre(k) = state_kalman(k-1);
%用上一時刻的最優估計值來作為對當前時刻的溫度的**
variance_pre(k) = variance_kalman(k-1)+q;
%**的方差為上一時刻溫度最優估計值的方差與高斯雜訊方差之和
%%計算卡爾曼增益
k(k) = variance_pre(k)/( variance_pre(k)+r ); 
%%結合當前時刻溫度計的測量值，對上一時刻的**進行校正，得到校正後的最優估計。由於是直接測量，故c為1.
state_kalman(k) = state_pre(k)+k(k)*(z(k)-state_pre(k)); 
variance_kalman(k) = (1-k(k))*variance_pre(k); 
%計算最終估計值的方差用於下一次迭代
end%繪圖相關。。。。。
fontsize=14;
linewidth=3;
figure();
plot(z,'k+'); %畫出溫度計的測量值
hold on;
plot(state_kalman,'b-','linewidth',linewidth) %畫出最優估計值
hold on;
plot(x*ones(sz),'g-','linewidth',linewidth); %畫出真實值
legend('溫度測量值', 'kalman估計值', '真實值');
xl=xlabel('時間(分鐘)');
yl=ylabel('溫度');
set(xl,'fontsize',fontsize);
set(yl,'fontsize',fontsize);
hold off;
set(gca,'fontsize',fontsize);
figure();
valid_iter = [2:n_iter]; % variance_pre not valid at step 1
plot(valid_iter,variance_kalman([valid_iter]),'linewidth',linewidth); %畫出最優估計值的方差
legend('kalman估計的誤差估計');
xl=xlabel('時間(分鐘)');
yl=ylabel('℃^2');
set(xl,'fontsize',fontsize);
set(yl,'fontsize',fontsize);
set(gca,'fontsize',fontsize);

「`

執行結果如下：

由結果可以看出來，雖然測量值雜訊很大，但kalman filter的**值還是逐漸逼近了真實溫度，並且估計值的方差逐漸收斂！

如上討論，經典的卡爾曼濾波只適用於線性且滿足高斯分布的系統，但實際工程中並不是這麼簡單，比如飛行器在水平運動時有可能伴隨著自身的自旋，這時就需要應用擴充套件卡爾曼濾波（ekf）來解決這種情況。等我研究完之後和大家分享。

卡爾曼濾波器學習筆記（上）

卡爾曼濾波器

卡爾曼濾波器學習筆記（一）

卡爾曼濾波器學習心得

卡爾曼濾波器學習筆記（上）

卡爾曼濾波器

卡爾曼濾波器學習筆記（一）

卡爾曼濾波器學習心得

相關推薦