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

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

[复制链接]

该用户从未签到

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

EDA365欢迎您登录!

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

x
   卡尔曼滤波实现简单,滤波效果好 ,下面分享一个基于卡尔曼滤波的matlab算法,数据全部为一维状态,本人弥补的详细备注,供爱好者研究学习。+ j, ?0 Q- q0 {
; N. p1 v2 f  t
%%%%%%%%%%%%%%%%%%
3 J  R# w* J5 f( @%功能说明:Kalman滤波用在一维温度数据测量系统中
) a2 E7 @" O' Y& b, k" Vfunction main
) v# y# z5 W+ u- ~$ n. A%%%%%%
8 Q' s: e+ V  W5 YN = 120;                                    %一共采样的点数,时间单位是分钟,可理解为实验进行了60分钟的测量
8 u9 `% _. X# B1 r, a, _& ECON = 25;                                %室内温度的理论值,在这个理论值的基础上受过程噪声会有波动/ X1 x0 C0 `& W, D
%ones(a,b) 产生a行b列值为1的矩阵" x, L$ T9 M# n" S# f
%zeros(a,b)产生a行b列值为0的矩阵
9 ^. |7 M4 D8 s, l' S) x" L2 }  n5 n4 [Xexpect = CON*ones(1,N);     %期望的温度是恒定的25度,但实际温度不可能会这样的$ X" {. ~+ R! r( X# y9 K6 H: e# s
+ ~5 z# s% m6 Q/ b7 U
X = zeros(1,N);                         %房间各时刻真实温度值
1 ~& [0 i' |$ [9 I: ^Xkf = zeros(1,N);                      %Kalman滤波处理的状态,也叫估计值; M6 ?; t$ [- b
Z = zeros(1,N);                         %温度计测量值- l  ]2 }- Q+ ?5 w/ [: Q
P = zeros(1,N);
6 L- ?6 Q' r( r! c. v%X(1)为数组的第一个元素
$ U9 r( K, b/ }4 A+ NX(1) = 25.1;     %假如初始值房间温度为25.1度' A: B( W/ j% P' P, J
P(1) = 0.01;     %初始值的协方差   (测量值 - 真实值)^2
) L/ t: P$ p9 ^8 v2 h$ W" A: c, P- H
8 Q5 a$ C3 c, v8 ?* [' C%产生0-1的随机数  模拟系统的随机数据2 X7 O! H, _( b$ k  @
Z = 24+rand(1,N)*10 - 5;       ) G& y! i4 @# b

  i0 W- k) Z) cZ(1) = 24.9;     %温度计测的第一个数据) L7 j8 B$ B, c, l* L
Xkf(1) = Z(1);  %初始测量值为24.9度,可以作为滤波器的初始估计状态噪声
. U. F. U: C9 s8 h
$ g2 |+ h3 L: i% x" fQ = 0;        %扰动误差方差(不存在扰动误差只有测量误差)* q1 A9 R  L. m. z. V' U
R = 0.25;        %测量误差方差* P9 a# F$ f+ J2 v, c5 V: v! s. x
%sqrt(Q)*randn(1,N)为产生方差为0.01的随机信号) J/ j9 _9 v6 r$ i* p0 l) L, [& E
%W为过程噪声+ D2 f' k; T/ v8 a/ }" `
%V为测量噪声$ f8 t8 U# r$ c6 P* C1 r: ?5 n
W = sqrt(Q)*randn(1,N); %方差决定噪声的大小
6 T  ^1 M1 y- |0 S+ g$ z& K: FV = sqrt(R)*randn(1,N);%方差决定噪声的大小( `9 ^5 S+ ]" ]0 R) f
%系统矩阵; r6 l. X+ t5 p9 I( B( y
%解释:因为该系统的数据都是一维的,故各变换矩阵都是1,原因自己找书理解
! i4 R" p9 [# i: W4 L: z6 a  RF = 1;     ( j% M+ p; x& `$ ]
G = 1;
6 S4 @: d8 N2 q: g+ T+ PH = 1;! O& _( I# ?& h6 v: J4 \" v
%eye产生m×n的单位矩阵 数值应该为1
( u4 c' p; S, H# ]/ T, g" tI = eye(1);  %本系统状态为一维
3 T, X7 L/ w" g" J* j& ~%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
- [# D/ @8 i4 \6 }- O6 @, G% s0 l%模拟房间温度和测量过程,并滤波
! y8 |' B# p. k9 P  R
3 A9 b4 m; V: W* s2 pfor k = 2:N2 A, _* I$ S9 J
    %第一步:随时间推移,房间真实温度波动变化
0 Q$ d. @9 v, h- B  o& [, {    %k时刻房间的真实温度,对于温度计来说,这个真实值是不知道的但是它的存在又是客观事实,读者要深刻领悟这个计算机模拟过程
$ \1 `1 w+ c6 _    X(k) = F*X(k - 1)+G*W(k - 1);%实际值为理想值叠加上扰动噪声8 R- ^/ C4 H' n7 o. L% |2 P8 o* n
    %第二步:随时间推移,获取实时数据/ H3 ?9 E) I8 w/ T% Z) J
    %温度计对K时刻房间温度的测量,Kalman滤波是站在温度计角度进行的,
8 C9 l3 t- S; f1 ?    %它不知道此刻的真实状态X(k),只能利用本次测量值Z(k)和上一次估计值Xkf(k)来做处理,其目标是最大限度地降低测量噪声R的影响
& k5 @0 @8 P0 b9 p    %尽可能的逼近X(k),这也是Kalman滤波目的所在
- U5 ~# M( E) ^$ v2 T$ b
4 ^/ Y6 y  ~- g* ]$ v# R    %修改本次函数! C7 o& s2 I% L1 t2 X
    %Z(k) = H*X(k)+V(k);                %测量值为实际值叠加上测量噪声
: L+ D7 y$ a3 p* Z. O+ M* [) s
( B5 T, O! W. Q  y/ c5 g    %第三步:Kalman滤波; y  ~% y3 [  t( F$ O
    %有了k时刻的观测Z(k)和k-1时刻的状态,那么就可以进行滤波了,$ C2 L( Z+ B0 Q
    %读者可以对照(3.36)到式(3.40),理解滤波过程
0 T5 W6 ~3 d8 h0 ?" _, n    X_pre = F*Xkf(k - 1);                        %状态预测          X_pre为上一次卡尔曼滤波值
9 X* V, D! ^; {8 U7 T) n    P_pre = F*P(k - 1)*F + Q;                %协方差预测   
7 w6 B$ I6 J0 f0 D    %inv()为求一个方阵的逆矩阵。
* f% A( t) L1 y    Kg = P_pre*inv(H * P_pre*H' + R);  %计算卡尔曼增益8 ]( h6 f6 b  @) m/ q3 @
    e = Z(k) - H*X_pre;                           %新息                 本次测量值与上次预测值之差2 V$ X- G" m3 R/ W4 O
    Xkf(k) = X_pre + Kg*e;                     %状态更新         本次预测值
. \; y1 V) g1 Z    P(k) = (I - Kg*H)*P_pre;                    %协方差更新
, E# E" e9 I& Z0 d; j0 `7 Iend
9 H! J& G% ?. S6 e%计算误差! C! f) F3 c0 p" E
Err_Messure = zeros(1,N);   %测量值与真实值之间的偏差
. `; Y! l9 u% w9 L/ I4 t5 pErr_Kalman = zeros(1,N);     %Kalman估计与真实值之间的偏差" M" N' N3 s* I; B$ x
for k = 1:N & X. L% g, W% y2 @) O3 I
    Err_Messure(k) = abs(Z(k) - X(k));    %abs()为求解绝对值3 c# h, S7 f- N! _. N0 f; k
    Err_Kalman(k) = abs(Xkf(k) - X(k));1 ?, K' }% B8 ], x( \
end
) e$ z! S1 l- p; y- Ct = 1:N;/ z4 k1 x; W7 b: J- A1 w/ s# ^5 Y" A+ D
figure   %画图显示
& z. m" M$ s' e1 K%依次输出理论值,叠加过程噪声(受波动影响)的真实值
1 j- {9 ~1 j$ g" b%温度计测量值,Kalman估计值0 P* _) S+ {' B( m  n
plot(t,Xexpect,'-b',t,X,'-r',t,Z,'-ko',t,Xkf,'-g*');
: O/ [4 l# ?( N7 W5 _legend('期望值','真实值','观测值','Kalman滤波值');
2 k/ C$ g( h' f6 ^$ lxlabel('采样时间/s');$ C" G. x6 o; u- J* P+ V+ Q
ylabel('温度值/C');% h8 s: ^4 A8 t$ Z/ e/ V
%误差分析图
) q* J: P6 `/ s' @4 C" F- u( B  n/ Efigure  %画图显示2 Q' i/ J% x0 u8 ?( S. Z  f7 B  p7 ?
plot(t,Err_Messure,'-b.',t,Err_Kalman,'-k*');5 n' n6 |, f4 Y: U7 |
legend('测量偏差','Kalman滤波偏差');
8 ]4 t: u( s1 W& ]  r( Gxlabel('采样时间/s');
) H, d" A: W" J( eylabel('温度偏差值/C');3 [9 a1 H! g9 l% d+ k) I9 F
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
# ]$ d- R+ E6 @. y7 A+ u" v& r

: n- T4 s4 b; ~# m2 x0 S
+ G8 T4 j* b2 ]
& l) m; w# e/ R6 n) H5 N1 m
( B% j& `3 n- ?1 U

3 L4 W7 R+ b7 `6 d  m5 T
您需要登录后才可以回帖 登录 | 注册

本版积分规则

关闭

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

EDA365公众号

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

GMT+8, 2025-10-29 10:20 , Processed in 0.156250 second(s), 23 queries , Gzip On.

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

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

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