找回密码
 注册
关于网站域名变更的通知
查看: 433|回复: 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)
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 {

该用户从未签到

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

本版积分规则

关闭

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

EDA365公众号

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

GMT+8, 2025-7-22 19:07 , Processed in 0.109375 second(s), 24 queries , Gzip On.

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

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

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