TA的每日心情 | 难过 2019-11-19 16:03 |
|---|
签到天数: 1 天 [LV.1]初来乍到
|
EDA365欢迎您登录!
您需要 登录 才可以下载或查看,没有帐号?注册
x
%Title卡尔曼滤波基本方程
( f( u6 D7 K# y# n: \. H%A1初始化
) t$ i! T+ u. p c1 x7 P
~" A' X$ n. A& m7 e. d! I. |$ u%A1.1参数初始化1 u/ q4 Y3 l# t; h) Q4 h9 P
N=200;
$ W+ W' f) v) n M
0 j" D3 j* Q( XXa=zeros(N,1);%预估值
) s% g. \$ p0 qX=zeros(N,1);%当前时刻最优估计值1 z; }1 v/ n. T9 k/ e! U" g
Pa=zeros(N,1);%Xa的方差矩阵5 b: G+ E0 i+ Z. x V5 v1 T: |
P=zeros(N,1);%X的方差矩阵
6 x$ F$ T' |; e1 |' B2 _Kg=ones(N,1);%卡尔曼增益! F& i, o, {4 q2 A5 I% s
# S7 s+ e' o; g2 z9 M. j7 P8 S; u
Z=zeros(N,1);%观测值. L" f" o3 l- J9 l0 w3 C. {2 O3 ~5 L* I
8 f) [1 D8 i) z! P3 Y6 e/ h, K( M
W=randn(N,1);%过程噪声
( o1 Y( [7 [2 e$ z. d, JstdW=std(W);%标准差
$ ^4 a6 Q6 C% b1 u7 F6 Y) g$ B, |varW=stdW.^2;%方差( \. G( Y8 b4 W! H" P
V=0.5*randn(N,1);%观测噪声; H- O9 x; c! ?4 w7 o
stdV=std(V);%标准差
' c6 v7 A% u( |) Q, O q& |$ }varV=stdV.^2;%方差
9 ^% O/ a8 |2 k2 H! c+ P @. ?: ]1 U9 N0 K# C
A=1;
; m `' X+ N4 s3 C- d* aH=1;
, I' N, O& y( c( a$ K; m J( W; r, u8 B/ O
%A1.2状态初始化$ t5 D% E& [3 A+ h( O
X(1)=23;%初始估计温度7 R: }" P: e3 Z2 h) I& F1 E
P(1)=10;%初始估计方差
& D6 O$ Z3 _! UZ(1)=25+V(1);
% r( Z6 p) E2 f* O5 wXa(1)=23+W(1);
) P; B. E, @$ |1 v) P5 F2 I, s$ K) p3 |4 l
%卡尔曼滤波
6 W2 r9 D' n, e% a" R4 @# _for k=2:N7 [( x7 A' i3 v e+ ]- p* h
%预估
3 c) K* Y& a b7 h, W& f Xa(k)=A*X(k-1);# C9 X. k9 T4 X! P/ {
Pa(k)=A*P(k-1)*A'+varW;
: v' B) _' V) D+ Y0 Y6 m9 X/ e$ u% b' h5 C
%校正3 R' \9 A% ]" U% _, `: ~: n" o
Kg(k)=Pa(k)*H'/(H*Pa(k)*H'+varV);* t9 N5 y* i) O
Z(k)=25+V(k);%25是真实温度值8 u; O, I! E2 f; M
X(k)=Xa(k)+Kg(k)*(Z(k)-H*Xa(k));. W0 q. q7 a. M) |" z
P(k)=(1-Kg(k)*H)*Pa(k);
+ L' r7 G/ x8 m2 c3 _end& E! ]3 E! x" @
9 e4 O+ I( g; ?+ j6 a# Xt=1:N;3 k' D$ g- K; Y5 b0 Q/ U' S
realVal=25*ones(N,1);
$ ~% \" ]7 y9 R9 T* z# w8 a4 z! n: c& U$ R2 a) G: F: ]+ O+ _
plot(t,realVal,'y',t,Z,'c',t,Xa,'b',t,X,'r');; y) N2 f, y; ]' o" K% ^. k
axis([0 N 20 30]); M/ u. K7 J. f) d
legend('真实值','观测值','预估值','估计值');/ @" @, j% Z) ~6 i+ f' v. X
grid on;
1 D( |8 f: Z. U- i* j6 q5 y8 }' g( H+ Q6 d+ m( m1 G, s
title('Kalman Filter');
' H, _0 Y2 k. t# u! ~! Y
& N. N- R; f; h/ o; j" A: ]2 |, `%realVal真实值;Z观测值;Xa预估值;X最优估计值;W过程噪声;V观测噪声
% o, M3 G2 S7 h' H# E% q3 D) s7 h9 ~- K
|
|