clear
n=200;%取200個數
w(1)=0;
w=randn(1,n);%產生乙個1×n的行向量,第乙個數為0,w為過程雜訊（其和後邊的v在卡爾曼理論裡均為高斯白雜訊）
x(1)=0;%狀態x初始值
a=1;%a為狀態轉移陣,此程式簡單起見取1
for k=2:n
x(k)=a*x(k-1)+w(k-1); %系統狀態方程,k時刻的狀態等於k-1時刻狀態乘以狀態轉移陣加雜訊（此處忽略了系統的控制量）
endv=randn(1,n);%測量雜訊
q1=std(v);
rvv=q1.^2;
q2=std(x);
rxx=q2.^2; %此方程未用到rxx
q3=std(w);
rww=q3.^2; %rvv、rww分別為過程雜訊和測量雜訊的協方差(此方程只取一組數方差與協方差相同)
c=0.2;
y=c*x+v;%量測方差,c為量測矩陣,同a簡化取為乙個數
p(1)=0;%初始最優化估計協方差
s(1)=0;%s(1)表示為初始最優化估計
for t=2:n
p1(t)=a.^2*p(t-1)+rww;%p1為一步估計的協方差,此式從t-1時刻最優化估計s的協方差得到t-1時刻到t時刻一步估計的協方差
b(t)=c*p1(t)/(c.^2*p1(t)+rvv);%b為卡爾曼增益,其意義表示為狀態誤差的協方差與量測誤差的協方差之比(個人見解)
s(t)=a*s(t-1)+b(t)*(y(t)-a*c*s(t-1));%y(t)-a*c*s(t-1)稱之為新息,是觀測值與一步估計得到的觀測值之差,此式由上一時刻狀態的最優化估計s(t-1)得到當前時刻的最優化估計s(t)
p(t)=p1(t)-c*b(t)*p1(t);%此式由一步估計的協方差得到此時刻最優化估計的協方差
endt=1:n;
plot(t,s,'r',t,y,'g',t,x,'b');%作圖,紅色為卡爾曼濾波,綠色為量測,藍色為狀態
%整體來說,此卡爾曼程式就是乙個迴圈迭代的過程,給出初始的狀態x和協方差p,得到下一時刻的x和p,迴圈帶入可得到一系列的最優的狀態估計值,此方法通常用於目標跟蹤和定位.
通俗解釋卡爾曼濾波
在學習卡爾曼濾波器之前，首先看看為什麼叫 卡爾曼 跟其他著名的理論 例如傅利葉變換，泰勒級數等等 一樣，卡爾曼也是乙個人的名字，而跟他們不同的是，他是個現代人！簡單來說，卡爾曼濾波器是乙個 optimal recursive data processing algorithm 最優化自回歸資料處理演...
卡爾曼濾波簡單解釋
卡爾曼濾波 以陀螺儀測量的角速度作為 值的控制量，加速度感測器測量的角度作為觀測值。下面程式中angle m為測量角度，gyro m為測量角速度，gyro m dt為控制量。以下程式是按卡爾曼濾波的五個公式來編寫的。x k k 1 a x k 1 k 1 b u k 1 p k k 1 a p k ...
通俗解釋卡爾曼濾波
假設你有兩個感測器，測的是同乙個訊號。可是它們每次的讀數都不太一樣，怎麼辦？取平均。再假設你知道其中貴的那個感測器應該準一些，便宜的那個應該差一些。那有比取平均更好的辦法嗎？加權平均。怎麼加權？假設兩個感測器的誤差都符合正態分佈，假設你知道這兩個正態分佈的方差，用這兩個方差值，此處省略若干數學公式 ...