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

转:Kalman滤波的Matlab仿真程序解读

[复制链接]

该用户从未签到

跳转到指定楼层
1#
发表于 2020-4-27 10:22 | 只看该作者 回帖奖励 |倒序浏览 |阅读模式

EDA365欢迎您登录!

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

x
最近因为要用到Kalman滤波器做东西,所以一直在学习这个东西,鉴于之前的仿真都是用matlab做的,所以呢,kalman滤波器的仿真程序也是用matlab编的。痛苦了几天,几天这个函数终于搞定了,具体的分析如下。

function [zx,zy]=xyKalmanFliter(A,H,Rw,Rv,Rw_c,Rv_c,x0,p0,y)
4 i- t4 \$ I3 D' V  J! N/ N%----------------输入参数--------------------------------------------------# a. |* W/ H6 T- g# H
%      A -- 系统矩阵/ Y1 C% }( d' ?+ ~) h4 B, U
%      H -- 观察矩阵& F4 N8 F9 a3 m  ^2 G' U
%     Rw -- 扰动向量
/ J7 P- V7 `, p%     Rv -- 测量噪声
' I+ w, D2 Z: ~+ R: p1 I  G%   Rw_c -- 扰动向量的协方差( O# A$ f1 s. l  r5 \4 N
%   Rv_c -- 测量噪声的协方差/ e) h& C7 O7 Z$ ~1 Z
%     x0 -- 节点初始位置向量(x,y)'
% V! ?5 y; N* N9 A/ @1 A0 X1 ^: [%     p0 -- 初试协方差阵
9 x- @* l4 T9 D1 X. D%      y -- 采样周期
' v5 Q: |) r( q3 \2 K! a%--------------------------------------------------------------------------

6 W  b( j! e6 b, P$ g* E
%--------------------------------------------------------------------------
: e' Z! m  D1 a$ {- j%   X(k) = A*X(k) + Rw(k)     噪声Q0 B: }; N" j. v4 s5 B
%   Z(k) = H*X(k) + Rv(k)     噪声R0 j& O) P2 p4 @

%  x(k|k-1) = A*x(k-1|k-1) + B*U(k)
* z0 O% h5 Q+ ~; J/ W%  P(k|k-1) = A*P(k-1|k-1)*A' + Q  n2 C! u) ~( g" U  p$ m
%  x(k|k) = x(k|k-1) + kg(k)*(z(k)-H*s(k|k-1))
$ S5 ]4 A2 ~7 f* p; }%  kg(k) = P(k|k-1)*H' / (H*P(k|k-1)*H' + R): i7 C! K/ _3 ]4 e
%  P(k|k) = (I-kg(k)*H)*P(k|k-1)7 `: ?4 f+ V6 m
%--------------------------------------------------------------------------

len = length( y );                                                        %获取采样点数

r = size(A,1);                                                            %获取系统矩阵A的行数  Q  |4 P5 P* l* |, r' c
I= eye( r(1) );                                                           %生成单位矩阵
" K6 {4 r) j3 ^9 ]9 |8 v5 n! V P1 = zeros( r(1),r(1) );                                                  %初始化协方差阵

%初始化节点位置,协方差阵
, d, K: L# u) ?4 z+ X X = x0;
5 w' Z9 R/ I, r: U/ W' I# Q P = p0;

len1 = size( H*X ,1);5 {5 d' Y" `8 q# d) c# q
for s=1:1:len
  _# D9 a9 g) N& R1 L, @     z1 = A*X+ Rw;                        % X(k) = A*X(k) + Rw(k)     协方差  Rw_c
5 ^' B4 z* _0 [' q# |1 @     zx(1:r(1),s) = z1(1:r(1)) ;    ) I$ ~. N& X3 ]& i% J! T
     z2 = H*X + Rv;                       % Z(k) = H*X(k) + Rv(k)     协方差  Rv_c& i/ ^- d7 G9 |
     zy(1:len1,s) = z2(1:len1 );
6 V: X% w* w; U8 X4 V; H, \    & X6 V" i: b  O! J: I+ A7 N
     P1 = A*P*A' + Rw_c;                  %  P(k|k-1) = A*P(k-1|k-1)*A' + Q
* r1 ~$ Y# [( i8 N; h" O+ j     K = P1*H'*inv( H*P1*H' + Rv_c );     %  kg(k) = P(k|k-1)*H' / (H*P(k|k-1)*H' + R)6 i3 q+ ?* {/ t: Q5 G: a6 n0 `
     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))  w) Q' M, f) _/ M# |9 H8 D
     P = ( I - K*H ) * P1;                %  P(k|k) = (I-kg(k)*H)*P(k|k-1)( t3 |7 L/ l4 T9 d4 M. o1 _! V
end


. I; I" }( ]0 W1 @6 `5 \return

原文地址:转:Kalman滤波的Matlab仿真程序解读作者:浩瀚


7 d, y% }; ]8 ]: [6 Q

该用户从未签到

2#
发表于 2020-4-27 13:27 | 只看该作者
Kalman滤波的Matlab仿真程序解读
您需要登录后才可以回帖 登录 | 注册

本版积分规则

关闭

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

EDA365公众号

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

GMT+8, 2025-7-24 16:30 , Processed in 0.125000 second(s), 23 queries , Gzip On.

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

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

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