找回密码
 注册
关于网站域名变更的通知
查看: 1769|回复: 3
打印 上一主题 下一主题

利用matlab实现卡尔曼滤波算法

[复制链接]

该用户从未签到

跳转到指定楼层
1#
发表于 2019-8-13 09:49 | 只看该作者 回帖奖励 |倒序浏览 |阅读模式

EDA365欢迎您登录!

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

x
   卡尔曼滤波实现简单,滤波效果好 ,下面分享一个基于卡尔曼滤波的matlab算法,数据全部为一维状态,本人弥补的详细备注,供爱好者研究学习。
  z+ t/ f6 |( h5 Y! U& K4 J

! v" v" v8 I; y' h%%%%%%%%%%%%%%%%%%
) I) }* H+ ^( {. W6 Z0 s%功能说明:Kalman滤波用在一维温度数据测量系统中7 K3 i0 ?' {, ?$ e& G$ X3 C- s) Y9 S
function main
, G0 a- M# h0 v) s8 @) \8 `%%%%%%; Z7 t8 z. t5 j" a
N = 120;                                    %一共采样的点数,时间单位是分钟,可理解为实验进行了60分钟的测量
9 L( H0 V) M" H7 D2 M% H9 q- J4 yCON = 25;                                %室内温度的理论值,在这个理论值的基础上受过程噪声会有波动4 q3 ?+ M! h: K) {$ |
%ones(a,b) 产生a行b列值为1的矩阵% Z; b) X  ]+ e4 \- |, ?
%zeros(a,b)产生a行b列值为0的矩阵6 M2 o" v$ d" I9 L- L$ X
Xexpect = CON*ones(1,N);     %期望的温度是恒定的25度,但实际温度不可能会这样的; l' J* p5 D3 M( [' j% h3 Y- l  R

: _! Y4 J+ D: W, w5 eX = zeros(1,N);                         %房间各时刻真实温度值! O6 S; n! a6 o# N
Xkf = zeros(1,N);                      %Kalman滤波处理的状态,也叫估计值4 J5 E8 I% Q9 g- l
Z = zeros(1,N);                         %温度计测量值( A  v( d/ @& |/ s
P = zeros(1,N);
: U% R* m& Q" I+ r! {%X(1)为数组的第一个元素
9 [0 Z5 L. h- P5 v9 p' H, I' pX(1) = 25.1;     %假如初始值房间温度为25.1度" d; x& h# J: d, m5 d# x
P(1) = 0.01;     %初始值的协方差   (测量值 - 真实值)^2
2 n& ?  M8 q( e
9 l0 W) ?" X, _5 N%产生0-1的随机数  模拟系统的随机数据9 e- H; g" `& J0 ?* H
Z = 24+rand(1,N)*10 - 5;      
( n7 m& m" C3 F) F1 H" U# Z4 i# ?' M0 _: h( o
Z(1) = 24.9;     %温度计测的第一个数据! L# s8 I9 }: C  t
Xkf(1) = Z(1);  %初始测量值为24.9度,可以作为滤波器的初始估计状态噪声
) l5 w+ T4 m+ l) N& m) g& P9 \4 I. f8 f
Q = 0;        %扰动误差方差(不存在扰动误差只有测量误差). u( N6 p$ ?5 d
R = 0.25;        %测量误差方差
8 B1 r/ Q, B1 x- v4 T%sqrt(Q)*randn(1,N)为产生方差为0.01的随机信号
3 p& J! L  }2 P, z3 ~- ?. m%W为过程噪声
  D1 B# d# ]8 }+ `%V为测量噪声
( A+ p7 {6 \% D2 U5 E8 M3 Y, rW = sqrt(Q)*randn(1,N); %方差决定噪声的大小
, {! Y% p( S/ Z* ?  oV = sqrt(R)*randn(1,N);%方差决定噪声的大小9 v! \- b8 h1 K
%系统矩阵
6 T+ z: ~2 b1 J/ Y* Z& w4 ~1 w: t3 Q%解释:因为该系统的数据都是一维的,故各变换矩阵都是1,原因自己找书理解  N, Q# E; x( H. r5 w+ g
F = 1;     
! Q: B% |+ t' D  ^G = 1;0 x5 o" X% J! _& V( A- f1 b4 m
H = 1;
( h0 n& T# o* @5 A! m- m8 A%eye产生m×n的单位矩阵 数值应该为1
4 o5 z# Z' x& CI = eye(1);  %本系统状态为一维6 U- I( i& r$ ~0 S9 P
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
. J% h( t, r, b1 q%模拟房间温度和测量过程,并滤波, b  Z9 W% L& F2 d' P; Q% m

( \# _) T+ t# t! s5 Hfor k = 2:N
" a/ c& O- W* G. B: Q    %第一步:随时间推移,房间真实温度波动变化
7 v3 r2 y+ Y- @8 A7 A- ~    %k时刻房间的真实温度,对于温度计来说,这个真实值是不知道的但是它的存在又是客观事实,读者要深刻领悟这个计算机模拟过程0 B1 K( v& r. m8 O! Y
    X(k) = F*X(k - 1)+G*W(k - 1);%实际值为理想值叠加上扰动噪声1 h8 l, C, [& S& r' `
    %第二步:随时间推移,获取实时数据6 v4 D( `+ T+ ^! v8 K
    %温度计对K时刻房间温度的测量,Kalman滤波是站在温度计角度进行的,1 Z  E5 G/ H3 _: F# |7 F# K
    %它不知道此刻的真实状态X(k),只能利用本次测量值Z(k)和上一次估计值Xkf(k)来做处理,其目标是最大限度地降低测量噪声R的影响: X4 h3 r2 r) m+ h6 y5 X+ u
    %尽可能的逼近X(k),这也是Kalman滤波目的所在
4 f/ O5 c# [9 K$ P6 ?) G* H* }3 u0 l8 X1 L
    %修改本次函数3 V2 q& \: ?9 h# E
    %Z(k) = H*X(k)+V(k);                %测量值为实际值叠加上测量噪声( n4 B( _! a/ @# s+ z- ^

! u3 G! ?! i# O9 ^! X    %第三步:Kalman滤波! p8 H5 A/ N; \8 S3 y6 _& W
    %有了k时刻的观测Z(k)和k-1时刻的状态,那么就可以进行滤波了,
! h% @9 Q/ i" F    %读者可以对照(3.36)到式(3.40),理解滤波过程
" u: g: Z* H& I4 S    X_pre = F*Xkf(k - 1);                        %状态预测          X_pre为上一次卡尔曼滤波值
% z% a8 f$ E1 E) R- `% L* s    P_pre = F*P(k - 1)*F + Q;                %协方差预测    . O# R3 u: K' [
    %inv()为求一个方阵的逆矩阵。
+ B% [9 h* ]0 J9 w% I    Kg = P_pre*inv(H * P_pre*H' + R);  %计算卡尔曼增益
- U+ @) c  f) A    e = Z(k) - H*X_pre;                           %新息                 本次测量值与上次预测值之差
4 R3 B9 m5 P. A$ w/ a) n$ s. _    Xkf(k) = X_pre + Kg*e;                     %状态更新         本次预测值
8 @7 f: H- E& w6 w  C) s( U- E    P(k) = (I - Kg*H)*P_pre;                    %协方差更新4 g; F/ ]& r: l' r
end& N: j& ?7 f6 v. R5 M
%计算误差/ {/ j( r9 X. s( Y$ Z
Err_Messure = zeros(1,N);   %测量值与真实值之间的偏差
7 ]. a0 P0 y: H/ t3 j% PErr_Kalman = zeros(1,N);     %Kalman估计与真实值之间的偏差! \4 J' j! p* O2 V+ K8 o
for k = 1:N
( Q3 @% b$ y8 c3 ]6 a% p    Err_Messure(k) = abs(Z(k) - X(k));    %abs()为求解绝对值
9 d9 k$ m8 c: k    Err_Kalman(k) = abs(Xkf(k) - X(k));( h: I3 ?6 r8 q6 {& Q6 |
end
" S$ T  E0 _& w; N% ~" }9 ]9 lt = 1:N;
1 ~3 ]6 ~( I% Sfigure   %画图显示
' M# ~+ N& L9 D4 b%依次输出理论值,叠加过程噪声(受波动影响)的真实值& j- E0 h/ j' E( w8 o! t
%温度计测量值,Kalman估计值1 i3 L  b# w1 |1 I. v6 B$ V
plot(t,Xexpect,'-b',t,X,'-r',t,Z,'-ko',t,Xkf,'-g*');8 n# i: z! y0 ~. ]: ?
legend('期望值','真实值','观测值','Kalman滤波值');0 g2 i" D' z- e, P/ G; s5 ~2 V5 S
xlabel('采样时间/s');2 I+ s1 K8 c. @
ylabel('温度值/C');9 {! L  Q1 x' F, l/ t; j5 t
%误差分析图  \) p+ C) [$ Q1 h! W, I8 ~
figure  %画图显示
  q4 [& j0 @5 ]6 b- D2 vplot(t,Err_Messure,'-b.',t,Err_Kalman,'-k*');9 o. u3 ?  _9 n7 I
legend('测量偏差','Kalman滤波偏差');2 v' O8 ?  s, w+ x3 R$ G
xlabel('采样时间/s');
7 N% G8 F/ b% y! k/ @, c2 kylabel('温度偏差值/C');
* l$ x; Z9 Y7 C" k3 n%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

2 w' L1 l0 a; r: T4 q% h( e9 E! ]' o  x

, H. J' ?! y7 Y8 U) }8 ~8 Z2 \; a

' W2 N$ R2 v1 z1 c/ v* u) I1 n7 A( r, @* r( o

4 o9 R3 x, f% M+ G3 Q( J- r7 y% G" P
您需要登录后才可以回帖 登录 | 注册

本版积分规则

关闭

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

EDA365公众号

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

GMT+8, 2025-10-5 10:15 , Processed in 0.125000 second(s), 23 queries , Gzip On.

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

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

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