EDA365电子论坛网
标题:
卡尔曼
[打印本页]
作者:
gaoxings
时间:
2020-7-30 13:26
标题:
卡尔曼
卡尔曼
$ A3 v2 b# T) M2 w$ [. j
" ^( R6 ?$ p' x3 m) L p& H
clear clc;
; _) |5 W: X& g
N=600;%采样点的个数
' E1 G: u. f/ u2 ~( W3 B& f
CON=25;%室内温度的理论值
9 k+ S0 u8 R+ W3 t
x=zeros(1,N);%用来记录温度的最优化估算值
5 }7 ~! ~9 p# s: U$ {% L# D
y=randn(1,N)+CON;%温度计的观测值，其中叠加了噪声
9 w# M6 n9 ~* h: n* ?$ J
x(1)=20;%为x(k)赋初值
& G3 W/ ]$ X) i% O9 ~; p
p(1)=2;%x(1)对应的协方差
# K2 i' W* |" s+ H" a' [) `
Q=cov(randn(1,N));%过程噪声的协方差
6 I- Q9 E0 v2 k+ Y( K' P- M
R=cov(randn(1,N));%测量噪声的协方差
L6 u& t' }; q3 \* q- n2 D0 }. z2 r
for k=2:N%循环里面是卡尔曼滤波的具体过程
N2 T- H/ l8 s
x(k)=x(k-1);
, W1 I- x3 H/ J4 u$ N* z0 r: U; f! z
p(k)=p(k-1)+Q;
A0 {% O0 T7 a( ]
Kg(k)=p(k)/(p(k)+R);%Kg为Kalman Gain,卡尔曼增益
& d, z% e" q8 G- N" m2 e/ Z6 |8 v
x(k)=x(k)+Kg(k)*(y(k)-x(k));
% E; i* ~) q6 R0 f0 N# b* ]
p(k)=(1-Kg(k))*p(k);
8 N' f9 Z X" m+ W- T8 o3 N
end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %这个模块起到平滑滤波作用
1 M* J) m! u: H- C- h! u! W
Filter_Width=10;%滤波器带宽
1 ]4 n9 G1 R& E4 V3 z" y/ s
Smooth_Result=zeros(1,N);%用来存放滤波后的各个采样点的值
4 e6 t' z) a+ w2 @" S, R
for i=Filter_Width+1:N
8 `6 Y0 ?9 F0 G2 e* V
Temp_Sum=0;
/ j' G% S! V+ X: R0 J* ~- P
for j=i-Filter_Width: (i-1)
, E% ]# s* u0 h8 m) x2 H5 f# n
Temp_Sum=x(j)+Temp_Sum;
9 n# V4 |3 W: m$ d1 B
end
J8 y' j( t/ p8 B, r q
Smooth_Result(i)=Temp_Sum/Filter_Width;%每一个点的采样值等于这个点之前的filter_width长度的采样点的平均值
: X4 [ U* y: U, P% ^4 G$ I- J- v
end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
3 n* e n& r) v1 C; Z, l3 [1 {( F
t=1:N;
' n+ P* z" t5 V. n
figure('Name','Kalman Filter Simulation','NumberTitle','off');
! \8 L# v, ?9 I$ D
expected_Value=zeros(1,N);
" r1 a7 Z( |/ `: l) D) {: E- S
for i=1:N
# u) d9 F0 a, B. h! I
expected_Value(i)=CON;
) C+ Z C6 s. i+ m3 G/ s. C
end
0 i% V& J$ O% @% s1 A, k! V- Z* f
plot(t,expected_Value,'-b',t,y,'-g',t,x,'-k',t,Smooth_Result,'-m');%依次输出理论值，叠加测量噪声的温度计测量值，
5 }' Z. @7 [/ |$ }+ X# S$ J1 H
legend('expected','measure','estimate','smooth result'); %经过kalman滤波后的最优化估算值，平滑滤波后的输出值
4 Y) L% K# ?1 b- X2 L0 E8 T9 W# ]/ `) r
xlabel('sample time');
. |( `+ q4 g+ @3 f9 [, u
ylabel('temperature'); title('Kalman Filter Simulation');
作者:
lilino
时间:
2020-7-30 14:24
程序给力！
欢迎光临 EDA365电子论坛网 (https://bbs.eda365.com/)
Powered by Discuz! X3.2