|
|
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 |
|