找回密码
 注册
8月份电巢直播计划
查看: 24|回复: 1

卡尔曼

[复制链接]

该用户从未签到

发表于 2020-7-30 13:26 | 显示全部楼层 |阅读模式

EDA365欢迎您登录!

您需要 登录 才可以下载或查看,没有帐号?注册

x
卡尔曼
/ @! W( F: d4 r$ [
/ c" Q7 o8 I7 Oclear clc;
6 f9 L7 u8 l- w! Y/ ]N=600;%采样点的个数  ! v$ H. Z3 y. w
CON=25;%室内温度的理论值
. t; y6 w/ ?% |+ l9 }x=zeros(1,N);%用来记录温度的最优化估算值  8 }! ~; g3 V+ F: S: I: M
y=randn(1,N)+CON;%温度计的观测值,其中叠加了噪声  P4 x# F: B$ ^& _
x(1)=20;%为x(k)赋初值
3 X7 _0 ?1 r$ K+ P3 Ip(1)=2;%x(1)对应的协方差0 R5 n' p' }! ~% y6 J# l
Q=cov(randn(1,N));%过程噪声的协方差4 I% h' e. F1 }- t' n
R=cov(randn(1,N));%测量噪声的协方差/ w% ^+ {" N1 Z) E: V" b
for k=2:N%循环里面是卡尔曼滤波的具体过程   
4 E1 M0 A3 ~  nx(k)=x(k-1);   ! E: a2 d. _4 R9 a( _2 G
p(k)=p(k-1)+Q;     - B& @, Y9 O( h, d
Kg(k)=p(k)/(p(k)+R);%Kg为Kalman Gain,卡尔曼增益     7 u! k1 x% t5 D; m" B: D* v
x(k)=x(k)+Kg(k)*(y(k)-x(k));     : |  h' S: H& J! ?1 x5 f7 V
p(k)=(1-Kg(k))*p(k);: H* W& G$ L7 S8 _- x, T! f
end  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %这个模块起到平滑滤波作用
% b7 v7 o- O/ j9 \  a' h. FFilter_Width=10;%滤波器带宽" }: i$ H0 ?  X; G2 C0 F  R( j0 X& B) s
Smooth_Result=zeros(1,N);%用来存放滤波后的各个采样点的值
. q2 \- }! R* z, U; j% rfor i=Filter_Width+1:N     
3 j. x$ y& e* I! DTemp_Sum=0;      4 W) ^6 q6 l" {2 H
for j=i-Filter_Width: (i-1)      
$ Q2 Z/ r( \& E5 K' o* [; w5 DTemp_Sum=x(j)+Temp_Sum;   5 x4 `6 `' B. T3 _- |% j1 C
end      
0 |2 a: ?% w1 e, nSmooth_Result(i)=Temp_Sum/Filter_Width;%每一个点的采样值等于这个点之前的filter_width长度的采样点的平均值$ P( k0 h9 Q1 o
end  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
2 Y3 ]8 s0 n& f. s& t: r! S9 ~* Ht=1:N;  
) n# m/ ~% r$ L3 kfigure('Name','Kalman Filter Simulation','NumberTitle','off');" {7 i1 X3 B1 ?4 X4 I4 o
expected_Value=zeros(1,N);
! Z1 a7 r1 C. w* R4 Z7 h) pfor i=1:N      4 A4 n4 J1 |  Z8 j4 A" t! z
expected_Value(i)=CON;, A8 @( W: s) c5 l5 J0 M' f" }
end
5 G0 E* q* {/ ]' @9 {% f0 c# c% Iplot(t,expected_Value,'-b',t,y,'-g',t,x,'-k',t,Smooth_Result,'-m');%依次输出理论值,叠加测量噪声的温度计测量值,
& Z' z' A; z8 {% y- C+ h& xlegend('expected','measure','estimate','smooth result');           %经过kalman滤波后的最优化估算值,平滑滤波后的输出值* K, l$ M! ~7 S$ q8 x
xlabel('sample time');
& x% H7 Y- a; v# y, {5 uylabel('temperature');  title('Kalman Filter Simulation');
您需要登录后才可以回帖 登录 | 注册

本版积分规则

关闭

推荐内容上一条 /1 下一条

EDA365公众号

关于我们|手机版|EDA365电子论坛网 ( 粤ICP备18020198号 )

GMT+8, 2020-8-14 08:28 , Processed in 0.078125 second(s), 20 queries , Gzip On.

深圳市墨知创新科技有限公司

地址:深圳市南山区科技生态园2栋A座805 电话:19925233282

快速回复 返回顶部 返回列表