EDA365欢迎您登录!
您需要 登录 才可以下载或查看,没有帐号?注册
x
最近因为要用到Kalman滤波器做东西,所以一直在学习这个东西,鉴于之前的仿真都是用matlab做的,所以呢,kalman滤波器的仿真程序也是用matlab编的。痛苦了几天,几天这个函数终于搞定了,具体的分析如下。function [zx,zy]=xyKalmanFliter(A,H,Rw,Rv,Rw_c,Rv_c,x0,p0,y)
: ]2 R' o8 s5 W4 x%----------------输入参数--------------------------------------------------
% H# T. Z# Q) p! w0 `% A -- 系统矩阵% C6 `& l- [( D4 t0 s
% H -- 观察矩阵) d0 Z8 r4 g; }6 r0 t, p
% Rw -- 扰动向量
( X2 v5 j; p" I& Y9 j0 v% Rv -- 测量噪声
* c' i0 F R# y( G% Rw_c -- 扰动向量的协方差& T! T/ p7 ~' u) Q
% Rv_c -- 测量噪声的协方差# `# w7 N& l1 ^, s6 G
% x0 -- 节点初始位置向量(x,y)'- ~& c$ M! g6 P5 \2 G
% p0 -- 初试协方差阵: F3 K: E( S' h9 d) k* @0 n$ O
% y -- 采样周期
7 W% f: \) d' S$ l%-------------------------------------------------------------------------- 3 W# ^3 H- |8 u! n
%--------------------------------------------------------------------------
! p: k2 G7 a9 ~+ Y9 e. b% X(k) = A*X(k) + Rw(k) 噪声Q; o! r' {; g4 v1 }. @
% Z(k) = H*X(k) + Rv(k) 噪声R9 x- { t$ G3 \( N( `
% x(k|k-1) = A*x(k-1|k-1) + B*U(k)/ {: }8 w- G0 w8 T2 s) @+ P* T
% P(k|k-1) = A*P(k-1|k-1)*A' + Q
) y1 C/ |% X# m7 h; z% x(k|k) = x(k|k-1) + kg(k)*(z(k)-H*s(k|k-1))
" i+ m! Q$ ?! @7 P: e+ I% kg(k) = P(k|k-1)*H' / (H*P(k|k-1)*H' + R)
: _& f& b* y( o3 O2 t& k% P(k|k) = (I-kg(k)*H)*P(k|k-1)
4 |0 ~/ G. _2 y7 |4 T1 V. }4 D0 T%-------------------------------------------------------------------------- len = length( y ); %获取采样点数 r = size(A,1); %获取系统矩阵A的行数
4 ?4 m3 @4 a- O+ u I= eye( r(1) ); %生成单位矩阵, b: P' W- ^( }0 s: n
P1 = zeros( r(1),r(1) ); %初始化协方差阵 %初始化节点位置,协方差阵 a- O" h1 ]1 J0 X' |! N) m- {
X = x0;* C2 B& Z P3 F- T" I5 l
P = p0; len1 = size( H*X ,1);4 t$ c; e% s, K H
for s=1:1:len
/ |; [5 q6 V x/ v7 Y z1 = A*X+ Rw; % X(k) = A*X(k) + Rw(k) 协方差 Rw_c, Z" Z6 { \$ [+ j
zx(1:r(1),s) = z1(1:r(1)) ; $ F- Y- ?# G$ a4 r _8 P
z2 = H*X + Rv; % Z(k) = H*X(k) + Rv(k) 协方差 Rv_c X5 P! D& l; y h; r6 C
zy(1:len1,s) = z2(1:len1 );+ C/ O( y) K0 A( x% a R6 X. o$ P
* g6 ?9 f0 o2 t- o
P1 = A*P*A' + Rw_c; % P(k|k-1) = A*P(k-1|k-1)*A' + Q
! a3 G7 }. u2 g. Q1 E$ m" z. h K = P1*H'*inv( H*P1*H' + Rv_c ); % kg(k) = P(k|k-1)*H' / (H*P(k|k-1)*H' + R)
8 x& d9 U- g; C% ?. d 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)); x" Y) g+ {* Q
P = ( I - K*H ) * P1; % P(k|k) = (I-kg(k)*H)*P(k|k-1)
5 }. u9 a0 \3 e2 v( x8 B( ` end / W2 J6 M4 ?- X& Z) v
return 原文地址:转:Kalman滤波的Matlab仿真程序解读作者:浩瀚
t* ~9 y; Y! X6 N |