簡單卡爾曼程式詳解

2021-07-23 19:53:07 字數 1156 閱讀 8619

>> 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 系統狀...