clear;clc;
f=[1 1;0 1];b=[0.5;1];h=[0 1];
x=[10; 10];%初始狀態
u=1;%加速度
p=[1 0;0 1];%初始協方差矩陣
q=[0.001 0;0 0.001];
z=1:1000;%測量值，勻加速運動
noise=15*randn(1,1000);%測量雜訊100個
z=z+noise;
r=1;
state=zeros(2,1000);
for i=1:1000
state(:,i)=x;
%根據上步最優值計算估計值
x_=f*x+b*u;
p_=f*p*f'+q;
%更新計算最優值
k=p_*h'/(h*p_*h'+r);
p=(eye(2)-k*h)*p_;
x=x_+k*(z(i)-h*x_);
endt=1:1000;
plot(t,state(1,t),'r.',t,state(2,t),'b')
hold on
plot(t,z);
legend('位移','速度')
小車為勻加速運動，速度測量值帶有雜訊（藍色），從上圖可以看出濾波得到的值比較好
卡爾曼濾波的核心思想是根據上一步計算得到的最優值對下一步的值進行**，得到乙個**值，再根據當前的觀測值計算乙個當前的最優值，不斷地更新係數k(與過程雜訊方差q和測量雜訊方差r相關，雜訊服從均值高斯分布，測量雜訊與感測器相關，因此k值決定了是更傾向於測量值還是**值)
學習卡爾曼濾波
偶然機會想學學 最起碼知道一下 卡爾曼濾波，以前學人工智慧課程學過濾波演算法，但是老師只是提了一下有這個東西，一直不知道如何用它來解決問題。1，參考資料 http www.cs.unc.edu welch kalman 裡面有乙個 an introduction to the kalman filt...
卡爾曼 卡爾曼濾波 1
今天主要介紹一下卡爾曼濾波器，所謂卡爾曼濾波器其實是一種最優化遞迴數字處理演算法 optimal recursive data processing algorithm 卡爾曼濾波器應用 既然我們有了測量儀器，這些測量儀器可以目標給出準確測量值。還需要卡爾曼濾波器進行估計嗎?下面解釋一下為什麼需要卡...
卡爾曼 基礎卡爾曼濾波
卡爾曼濾波器是一種基礎 定位演算法。原理非常簡單易懂。核心過程可以用乙個圖說明 本質上就是這兩個狀態過程的迭代，來逐步的準確定位。更新 更具感測器獲取到比較準確的位置資訊後來更新當前的 問位置，也就是糾正 的錯誤。你可能要問為什麼有感測器的資料了還要進行更新？因為在現實世界中感測器是存在很多雜訊干擾...