>> 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，迴圈帶入可得到一系列的最優的狀態估計值，此方法通常用於目標跟蹤和定位。
>>
>>
卡爾曼濾波簡單解釋
卡爾曼濾波 以陀螺儀測量的角速度作為 值的控制量，加速度感測器測量的角度作為觀測值。下面程式中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 ...
卡爾曼濾波簡單理解
卡爾曼濾波是對系統的狀態 如汽車的速度 加速度等 進行最優估計，因為觀測資料中包含雜訊和干擾，所以最優估計也可看作是濾波過程。下面是卡爾曼濾波的兩個例子。例1 估計發動機內部溫度 如果想要測量火箭發動機內部的溫度，可以將感測器放入發動機內部測出內部溫度tin t ti n 但是感測器在內部會損壞無法...
卡爾曼濾波程式matlab 解釋
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 系統狀...