设为首页收藏本站|繁體中文 快速切换版块

 找回密码
 立即加入
搜索
查看: 626|回复: 2

扩展卡尔曼滤波小程序

[复制链接]
  • TA的每日心情
    郁闷
    2018-5-7 15:44
  • 签到天数: 10 天

    连续签到: 1 天

    [LV.3]偶尔看看II

    累计签到:10 天
    连续签到:1 天
    发表于 2017-4-5 11:56:50 | 显示全部楼层 |阅读模式
    电子图书
    电子图书名: 扩展卡尔曼滤波小程序
    编者: H
    内容简介: 简单的卡尔曼滤波程序
    所属专业方向: matlab
    出版社:
    来源:

    马上加入,结交更多好友,共享更多资料,让你轻松玩转电力研学社区!

    您需要 登录 才可以下载或查看,没有账号?立即加入

    ×
    % function  EXT Kalman Filtering
    / Y$ P6 C/ x* S$ }4 ~) l% vx[n]=vx[n-1]+ux[n]* B6 a5 W9 {, j8 R7 C
    % vy[n]=vy[n-1]+uy[n]
    - v$ M' k3 H, h) P3 L2 V9 |% rx[n]=rx[n-1]+vx[n-1]*t
    2 y. y+ d# a# ^4 [, w6 ^% ry[n]=ry[n-1]+vy[n-1]*t
    8 d, R, J+ o( b, D- C" k$ {* t% s[n]=[rx[n];ry[n];vx[n];vy[n]]
    , {# \  ?4 _. S  g! u% A=[1 0 t 0;0 1 0 t;0 0 1 0;0 0 0 1]
    7 d, Y, V% F/ ^/ u8 u' {& ]4 [/ `7 Y% s[n-1]=[rx[n-1];ry[n-1];vx[n-1];vy[n-1]]
    # T7 |; e$ |' G9 r' E% u[n]=[0;0;ux[n];uy[n]]5 `9 e0 ~; ^4 N; F0 a2 h$ w
    %       s[n]=As*[n-1]+u[n]7 a5 Y0 m' c4 V9 N5 u& [* z) f
    %       x[n]=h(s[n])+w[n]& b) A% g$ }" `2 S$ [% X/ E

    - p' S, J- O0 \% S% initialization6 [. {( L- O1 [' L: o3 F
    clear;
    2 p+ a1 D8 P8 ~/ cn=100;& D3 p0 a. d6 ]* S( C7 b7 a% ?
    A=[1 0 1 0;0 1 0 1;0 0 1 0;0 0 0 1];
    ; i& A$ p0 R6 y/ o$ q7 s: X  orx=zeros(1,n); % 实际位置3 J. W  h8 M# _8 Z
    ry=zeros(1,n); % 实际位置1 n. ]$ u) {/ r1 u
    vx=zeros(1,n); % 实际速度. o* y) K# V! t  U) T" W! x
    vy=zeros(1,n); % 实际速度
    & @9 _# K5 k  m2 r! J% @rxe=zeros(1,n); % 精确位置
    3 e: T6 w0 @5 R5 i; ]8 Crye=zeros(1,n); % 精确位置
    9 ?4 h) G- ~9 r, m8 v" v) s8 ]vxe=zeros(1,n); % 精确速度0 {( A$ I! V- U) Q$ P; @
    vye=zeros(1,n); % 精确速度
    % ]+ I* E+ I. D( O, z/ lrx(1,1)=10; % 位置初始
    0 [. T8 O2 [  Ory(1,1)=-5; % 位置初始
    8 ]& F5 b1 N( \' M( j! \vx(1,1)=-0.2; % 速度初始$ n/ H9 S) d8 }' E+ Y6 }  _
    vy(1,1)=0.2; % 速度初始
    7 @8 l' l6 d$ B) \+ Trxe(1,1)=10;
    9 e% J0 g9 J. `+ A) H% W6 r; X- ~rye(1,1)=-5;
    * m* \" [8 j6 u  K' v' Z/ uvxe(1,1)=-0.2;
    " B" w1 ?/ ]1 H# mvye(1,1)=0.2;5 i. G, ~3 w- V; J
    s=[rx;ry;vx;vy];
    7 T( r" e/ t  w, G) U, @se=[rxe;rye;vxe;vye];& S1 B) x8 i$ x. T
    r=zeros(1,n);0 A, g* I( ]6 O9 N
    b=zeros(1,n);
    . n" @# s  ]* E3 y8 i: c  J% r& Jrn=zeros(1,n);
    4 z5 U- l4 q3 K( Kre=zeros(1,n);9 i; ^0 B, ^6 i  n2 ]' p
    bn=zeros(1,n);
    % s$ p1 j( s, |5 O" Mx=[rn;bn];; j- `) C! b2 L: a4 j5 d
    H=zeros(2,4,n);
    8 [# A) }8 c1 _. Y# C6 I# e; n/ A$ [3 f# M5 H
    ux=sqrt(0.00001)*randn(1,n);7 W# z) k* x9 {- O" E+ \$ S
    uy=sqrt(0.00001)*randn(1,n);
    ' a0 W6 a4 y+ \- o0 kwr=sqrt(0.001)*randn(1,n);0 B$ u" a$ F- Z0 \8 t1 ^
    wb=sqrt(0.0001)*randn(1,n);
    % Z, w$ b" P) n
    & X  f; G% L5 z5 D. r# Y9 VQ=[0 0 0 0;0 0 0 0;0 0 cov(ux) 0;0 0 0 cov(uy)]; % 过程噪声矩阵' x- w6 _- `6 Q9 p2 g" b
    C=[cov(wr) 0; 0 cov(wb)]; % 观测噪声矩阵1 Z, }# G6 q; R  t

    ; T; ~3 d2 _/ n! K( z& c& ?- Bss=zeros(4,n);% 精确值# c, V$ _+ \! p' O( J
    sn=zeros(4,n); % 估计修正值; V' Z3 I4 d+ T3 Z# B# M- X* ]
    sn(:,1)=sn(:,1)+[15;5;0;0]; % 估计修正初始值  @+ F. n3 @8 \; ^
    ss=zeros(4,n); % 一步预测! K7 U2 t0 t: [* d, x& `' B/ p* R
    hs=zeros(2,n); % 观测值预测
    9 q3 A6 O9 W+ z0 u- cinn=zeros(2,n); % 新息, n) C% ^4 E7 x3 }
    M=zeros(4,4,n); % 估计均方误差
    4 M0 s$ J7 N5 T! M. u9 PM(:,:,1)=M(:,:,1)+[1 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1]; % 估计均方误差初始值' b; d8 p% y3 J0 ?
    MM=zeros(4,4,n); % 一步预测均方误差$ ~6 `% O, T# v  N1 n# D9 A( E
    K=zeros(4,2,n); % 卡尔曼增益
    " Z; z" I3 m# W& j
    8 H7 M6 i. @% A; f6 Y) Y% w& [% kalman filtering
    2 d2 n5 m- m  o8 |0 }6 t% y  s3 d8 {8 j4 b1 K6 k
    for t=1:n-1
    . o0 K/ v# C, O3 ^%            s[n]=As*[n-1]+u[n]4 ?; M. B# Z' C4 |
    %            x[n]=h(s[n])+w[n]
    3 I. O' C6 t& e6 [) w
    6 o, m# T. M9 b" T. S9 v1 }    vx(1,t+1)=vx(1,t)+ux(1,t+1); % 速度过程方程
    $ o3 P9 O# p4 t+ p: C2 L4 {    vy(1,t+1)=vy(1,t)+uy(1,t+1); % 速度过程方程
    1 z, h& x/ c+ O4 v9 J5 `# B1 ?, d    rx(1,t+1)=rx(1,t)+vx(1,t); % 位置过程方程- E! e( P+ `3 Z  m3 V) L7 x+ N
        ry(1,t+1)=ry(1,t)+vy(1,t); % 位置过程方程* J1 P% t' P8 Q4 g) `* P) T
        s(:,t+1)=[rx(1,t+1);ry(1,t+1);vx(1,t+1);vy(1,t+1)]; % 过程矩阵,维数4*1% A. l6 x$ d% i7 q3 R

    ! S0 r* N0 q! z  m  l4 O    vxe(1,t+1)=vxe(1,t); % 理论精确值
    6 Q3 `0 D( b& |3 H" k    vye(1,t+1)=vye(1,t); % 理论精确值
    2 w  ~  S2 V1 r! s8 q8 k$ l/ z, b9 m. p    rxe(1,t+1)=rxe(1,t)+vxe(1,t); % 理论精确值
    & F$ c6 g5 W( d) W8 A$ ~5 s" W6 S" n    rye(1,t+1)=rye(1,t)+vye(1,t); % 理论精确值
    , ^5 S; a0 E$ y& A# T  n    se(:,t+1)=[rxe(1,t+1);rye(1,t+1);vxe(1,t+1);vye(1,t+1)]; % 过程矩阵,维数4*1
    0 l( d! N+ m  M$ J' ?5 l& J( ]* _) ^3 b3 h) C9 W' o7 J4 s
        r(1,t+1)=(rx(1,t+1)^2+ry(1,t+1)^2)^(1/2); % 理论测量距离(含测量误差)
    " j# X- @" G0 v. x    b(1,t+1)=atan(ry(1,t+1)/rx(1,t+1)); % 理论测量方位(含测量误差)
    ; r- a. m5 Z8 A    rn(1,t+1)=r(1,t+1)+wr(1,t+1); % 实际测量距离# U& F7 ~/ y1 s% P: R. O4 d
        re(1,t+1)=(rxe(1,t+1)^2+rye(1,t+1)^2)^(1/2); % 理论测量距离(含测量误差)
    % A  Q! y0 K4 M6 X4 [4 s    bn(1,t+1)=b(1,t+1)+wb(1,t+1); % 实际测量方位( T" @# y& y9 C+ k
        x(:,t+1)=[rn(1,t+1);bn(1,t+1)]; % 观测矩阵,维数2*1
    & U8 p. G& ?4 e2 |1 o7 b
      {( `- |" z, O5 {6 K    % 对观测方程求导,得到雅可比矩阵H,维数2*4
    ( w) P6 i; ^: B    H(:,:,t+1)=[rx(1,t+1)/((rx(1,t+1)^2+ry(1,t+1)^2)^(1/2)) ry(1,t+1)/((rx(1,t+1)^2+ry(1,t+1)^2)^(1/2)) 0 0;-ry(1,t+1)/(rx(1,t+1)^2+ry(1,t+1)^2) rx(1,t+1)/(rx(1,t+1)^2+ry(1,t+1)^2) 0 0];
    4 `5 p8 t& u* x$ ?" C; h* v
    1 s+ v# C* S: E. p6 ]
      d# l( P) K0 d" T    MM(:,:,t+1)=A*M(:,:,t)*A'+Q; % 一步预测均方误差5 ^5 S9 C. z) }& V6 ?+ [" U
        K(:,:,t+1)=MM(:,:,t+1)*H(:,:,t+1)'/(C+H(:,:,t+1)*MM(:,:,t+1)*H(:,:,t+1)'); % 卡尔曼增益
    7 F% E& C3 T: K0 y    M(:,:,t+1)=(eye(4)-K(:,:,t+1)*H(:,:,t+1))*MM(:,:,t+1);% 估计均方误差" x6 T! d" C, M7 u0 E0 X  `3 \
        ss(:,t+1)=A*sn(:,t); % 一步预测) B6 s' j2 {$ f; @% J: |
        hs(:,t+1)=[((ss(1,t+1))^2+(ss(2,t+1))^2)^(1/2);atan(ss(2,t+1)/ss(1,t+1))]; % 观测值预测8 b. u0 q# K4 r& b
        inn(:,t+1)=x(:,t+1)-hs(:,t+1); % 新息  l) Z2 R2 g. `# Z# }* M; _# W
        sn(:,t+1)=ss(:,t+1)+K(:,:,t+1)*inn(:,t+1); % 预测修正
    ! O& r0 e8 `2 r, ^- j$ Wend' k  w, |/ M# x1 D' |- f+ f" Q
    6 v' o8 e" P1 o
    subplot(2,1,1);1 C8 l2 E+ j! Y3 h% x' W8 e
    plot(sn(1,:),sn(2,:)); % kalman估计值
    9 Z' I4 x( G& E3 Jhold on;
    % `; m2 [% ?) |plot(se(1,:),se(2,:),'-r'); % 理论精确值
    6 e5 ?  D. A8 C; A, m8 {8 n0 Q# e4 |4 J0 X; W% E
    subplot(2,1,2);
    4 _; C1 f; D! S/ G; vplot(rn); % 实际测量距离
    - C: H5 g, m* d$ w) mhold on;
    1 C; W9 S1 c! o1 Gplot(re,'-g'); % 理论精确值8 C- F5 K: S" ~, G/ \' z8 E/ x: ^
    5 p2 f" |7 O* g
    "真诚赞赏,手留余香"
    还没有人打赏,支持一下
    楼主热帖
    帖文化:【文明发帖 和谐互动】 社区精神:【创新、交流、互助、共享】
  • TA的每日心情
    郁闷
    2018-5-7 15:44
  • 签到天数: 10 天

    连续签到: 1 天

    [LV.3]偶尔看看II

    累计签到:10 天
    连续签到:1 天
     楼主| 发表于 2017-4-5 11:58:00 | 显示全部楼层
    这是一个位置和速度跟踪的小程序,感兴趣的可以看一下
    "真诚赞赏,手留余香"
    还没有人打赏,支持一下
    [发帖际遇]: 帮助老师傅注册上研学论坛,HYSYYRPS受奖励威望5 点. 幸运榜 / 衰神榜
    帖文化:【文明发帖 和谐互动】 社区精神:【创新、交流、互助、共享】
    回复 推荐 踩下

    使用道具 举报

  • TA的每日心情
    郁闷
    2018-5-7 15:44
  • 签到天数: 10 天

    连续签到: 1 天

    [LV.3]偶尔看看II

    累计签到:10 天
    连续签到:1 天
     楼主| 发表于 2017-4-5 11:59:36 | 显示全部楼层
    有没有同学是电力系统方向的,有的话,有没有做电力系统状态估计方向的,可以交流下
    "真诚赞赏,手留余香"
    还没有人打赏,支持一下
    帖文化:【文明发帖 和谐互动】 社区精神:【创新、交流、互助、共享】
    回复 推荐 踩下

    使用道具 举报

    您需要登录后才可以回帖 登录 | 立即加入

    本版积分规则

    招聘斑竹

    小黑屋|手机版|APP下载(beta)|Archiver|电力研学网 ( 赣ICP备12000811号-1|赣公网安备36040302000210号 )|网站地图

    GMT+8, 2025-8-13 06:58

    Powered by Discuz! X3.5 Licensed

    © 2001-2025 Discuz! Team.

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