%房間當前溫度真實值為24度，認為下一時刻與當前時刻溫度相同，誤差為0.02度（即認為連續的兩個時刻最多變化0.02度）。
%溫度計的測量誤差為0.5度。
%開始時，房間溫度的估計為23.5度，誤差為1度。
clear all;
close all;
% intial parameters
n_iter =
500;
%計算連續n_iter個時刻
sz =
[n_iter,1]
;% size of array. n_iter行，1列
x =24
;% 溫度的真實值
q =4e-4
;% 過程方差， 反應連續兩個時刻溫度方差。更改檢視效果4e-4
r =0.25
;% 測量方差，反應溫度計的測量精度。更改檢視效果0.25
z = x +
sqrt
(r)*
randn
(sz)
;% z是溫度計的測量結果，在真實值的基礎上加上了方差為0.25的高斯雜訊。
% 對陣列進行初始化
xhat=
zeros
(sz)
;% 對溫度的後驗估計。即在k時刻，結合溫度計當前測量值與k-
1時刻先驗估計，得到的最終估計值
p=zeros
(sz)
;% 後驗估計的方差
xhatminus=
zeros
(sz)
;% 溫度的先驗估計。即在k-
1時刻，對k時刻溫度做出的估計（不准）
pminus=
zeros
(sz)
;% 先驗估計的方差
k=zeros
(sz)
;% 卡爾曼增益，反應了溫度計測量結果與過程模型（即當前時刻與下一時刻溫度相同這一模型）的可信程度
% intial guesses
%xhat最優估計
xhat(1
)=23.5
;%溫度初始估計值為23.5度p(
1)=1
;%誤差方差為1（最優估計的方差）
for k =
2:n_iter
% 時間更新（**）
xhatminus
(k)=
xhat
(k-1);
%用上一時刻的最優估計值來作為對當前時刻的溫度的**--
--獲得當前時刻的**
%（公式二：）（當前）先驗估計的方差=上一時刻最優估計的方差+過程方差
pminus
(k)=
p(k-1)
+q;%**的方差為上一時刻溫度最優估計值的方差與過程方差之和--
--獲得當前**的方差
% 測量更新（校正）
%（公式三：）（當前）卡爾曼增益=**方差/
(**方差 + 測量方差)
k(k)
=pminus
(k)/
(pminus
(k)+r )
;%計算卡爾曼增益--
--**方差/
(**方差 + 測量方差)
%（公式四：）（當前）當前**的最優=當前**+卡爾曼增益*（當前測量值-當前**）
xhat
(k)=
xhatminus
(k)+
k(k)*(
z(k)
-xhatminus
(k))
;%結合當前時刻溫度計的測量值，對上一時刻的**進行校正，得到校正後的最優估計。該估計具有最小均方差
%（公式五：）（當前）最優估計值的方差=（1
-卡爾曼增益）*先驗估計
p(k)=(
1-k(k))*
pminus
(k);
%計算最終估計值的方差
endfontsize=14;
linewidth=3;
figure()
;plot
(z,'k+');
%畫出溫度計的測量值
hold on;
plot
(xhat,
'b-'
,'linewidth'
,linewidth)
%畫出最優估計值
hold on;
plot
(x*ones
(sz)
,'g-'
,'linewidth'
,linewidth)
;%畫出真實值
legend
('溫度計的測量結果'
,'後驗估計'
,'真實值');
xl=xlabel
('時間(分鐘)');
yl=ylabel
('溫度');
set(xl,
'fontsize'
,fontsize)
;set
(yl,
'fontsize'
,fontsize)
;hold off;
set(gca,
'fontsize'
,fontsize)
;figure()
;valid_iter =[2
:n_iter]
;% pminus not valid at step 1
plot
(valid_iter,p(
[valid_iter]),
'linewidth'
,linewidth)
;%畫出最優估計值的方差
legend
('後驗估計的誤差估計');
xl=xlabel
('時間(分鐘)');
yl=ylabel
('℃^2');
set(xl,
'fontsize'
,fontsize)
;set
(yl,
'fontsize'
,fontsize)
;set
(gca,
'fontsize'
,fontsize)
;
#include
"opencv2/video/tracking.hpp"
#include
"opencv2/highgui/highgui.hpp"
#include
using
namespace cv;
static
inline point calcpoint
(point2f center,
double r,
double angle)
static
void
help()
intmain
(int
,char**
)if(code ==
27|| code ==
'q'|| code ==
'q')
break;}
return0;
}
卡爾曼 卡爾曼濾波 1
今天主要介紹一下卡爾曼濾波器，所謂卡爾曼濾波器其實是一種最優化遞迴數字處理演算法 optimal recursive data processing algorithm 卡爾曼濾波器應用 既然我們有了測量儀器，這些測量儀器可以目標給出準確測量值。還需要卡爾曼濾波器進行估計嗎?下面解釋一下為什麼需要卡...
卡爾曼 基礎卡爾曼濾波
卡爾曼濾波器是一種基礎 定位演算法。原理非常簡單易懂。核心過程可以用乙個圖說明 本質上就是這兩個狀態過程的迭代，來逐步的準確定位。更新 更具感測器獲取到比較準確的位置資訊後來更新當前的 問位置，也就是糾正 的錯誤。你可能要問為什麼有感測器的資料了還要進行更新？因為在現實世界中感測器是存在很多雜訊干擾...
卡爾曼濾波
卡爾曼濾波演算法 首先引入乙個離散控制過程的系統，用乙個線性隨機微分方程來描述 x k a x k 1 b u k w k 系統的測量值 z k h x k v k x k 是k時刻的系統狀態，u k 是k時刻對系統的控制量。a和b是系統引數，對於多模型系統，他們為矩陣。z k 是k時刻的測量值，h...