EDA365欢迎您登录!
您需要 登录 才可以下载或查看,没有帐号?注册
x
最近因为要用到Kalman滤波器做东西,所以一直在学习这个东西,鉴于之前的仿真都是用matlab做的,所以呢,kalman滤波器的仿真程序也是用matlab编的。痛苦了几天,几天这个函数终于搞定了,具体的分析如下。function [zx,zy]=xyKalmanFliter(A,H,Rw,Rv,Rw_c,Rv_c,x0,p0,y)
9 F3 ?; X+ T4 B# x2 f+ k( ~/ b- I%----------------输入参数--------------------------------------------------
8 n6 H3 Q6 ^" _+ J1 ^! i% A -- 系统矩阵 |* ^; K$ P. Y# k8 C5 L% x
% H -- 观察矩阵
. }- R5 s* P% V% f+ @7 ^- C% Rw -- 扰动向量3 R: O ]2 @8 a, y( I$ f
% Rv -- 测量噪声
- | f) U) D2 F K% Rw_c -- 扰动向量的协方差" q# A' g6 z. V1 p
% Rv_c -- 测量噪声的协方差
# ?5 m* w# s0 O! J+ p. B$ g2 j% x0 -- 节点初始位置向量(x,y)'; u( |( g V4 f9 C
% p0 -- 初试协方差阵( j& }! L" Z) v1 B4 D/ w7 t
% y -- 采样周期2 v7 v- I; b9 J" }
%--------------------------------------------------------------------------
9 d0 E& m$ X2 {%--------------------------------------------------------------------------9 x, F# C4 ]2 [5 j7 r9 F/ e
% X(k) = A*X(k) + Rw(k) 噪声Q7 M( S! a/ p7 E$ S! W
% Z(k) = H*X(k) + Rv(k) 噪声R3 b' O# y+ M* z0 X- t( [# t D
% x(k|k-1) = A*x(k-1|k-1) + B*U(k)
/ T/ W' R w! I( A1 j; ^ s% P(k|k-1) = A*P(k-1|k-1)*A' + Q
" ~3 |# [4 X. a& p% x(k|k) = x(k|k-1) + kg(k)*(z(k)-H*s(k|k-1))6 d+ U% e$ C A7 }" V+ ?
% kg(k) = P(k|k-1)*H' / (H*P(k|k-1)*H' + R)" @" r/ A* Y; V2 Y1 j5 `' S
% P(k|k) = (I-kg(k)*H)*P(k|k-1)
3 a3 c7 a0 `1 l9 W- ~8 v( l# n%-------------------------------------------------------------------------- len = length( y ); %获取采样点数 r = size(A,1); %获取系统矩阵A的行数/ X( A9 ]" G5 L- H! p4 Z
I= eye( r(1) ); %生成单位矩阵
' n7 r9 g! m/ n/ J. f8 R P1 = zeros( r(1),r(1) ); %初始化协方差阵 %初始化节点位置,协方差阵9 N7 _7 E# W. c' s" T4 u0 q
X = x0;: [; T! z6 m/ z. [# j6 P1 C
P = p0; len1 = size( H*X ,1);
9 o c% D0 y/ u' A! z" P) h+ e for s=1:1:len, R, q, J. l7 o. @8 ~ U$ y
z1 = A*X+ Rw; % X(k) = A*X(k) + Rw(k) 协方差 Rw_c' V9 ?, v! P7 a% B1 k* \0 V0 L0 N
zx(1:r(1),s) = z1(1:r(1)) ; # \; }# {7 {, G9 X. k) s3 ^
z2 = H*X + Rv; % Z(k) = H*X(k) + Rv(k) 协方差 Rv_c
6 t5 P7 F/ \* ` zy(1:len1,s) = z2(1:len1 );8 P2 C% o( O4 b& ^9 I- c& L+ A: V
$ l! B$ w, e/ L, ? Z! T P1 = A*P*A' + Rw_c; % P(k|k-1) = A*P(k-1|k-1)*A' + Q
. t% j6 u" h! U' l K = P1*H'*inv( H*P1*H' + Rv_c ); % kg(k) = P(k|k-1)*H' / (H*P(k|k-1)*H' + R)
, G$ y3 \/ ?8 R X = A*X + K*( zy(1:len1,s) - H*A*X ); % x(k|k) = x(k|k-1) + kg(k)*(z(k)-H*s(k|k-1))
% w9 e. x5 s" d9 i P = ( I - K*H ) * P1; % P(k|k) = (I-kg(k)*H)*P(k|k-1)
2 o0 c' f& q0 L: x end
; \- n3 o* v( i! p- \return
原文地址:转:Kalman滤波的Matlab仿真程序解读作者:浩瀚
& Z6 o; ^) O1 { |