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