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

 找回密码
 立即加入
搜索
查看: 644|回复: 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* F: r5 f* m  r5 G
    % vx[n]=vx[n-1]+ux[n]
    7 z2 V/ ~% x, G- q7 B% vy[n]=vy[n-1]+uy[n]2 f1 c% c% I$ a& i) ]' R. \4 ?( r
    % rx[n]=rx[n-1]+vx[n-1]*t
    ; a- b  d3 u; H! k% ry[n]=ry[n-1]+vy[n-1]*t& e5 ]2 {1 P/ g/ _' p. Y: k
    % s[n]=[rx[n];ry[n];vx[n];vy[n]]
    - e+ _: n# v4 d  _% A=[1 0 t 0;0 1 0 t;0 0 1 0;0 0 0 1]
    6 G) g" _, q5 D% `3 i1 s  I% s[n-1]=[rx[n-1];ry[n-1];vx[n-1];vy[n-1]]+ B. C0 X9 P0 {2 F: y; ~- ~
    % u[n]=[0;0;ux[n];uy[n]]6 O: G! ^: z( f/ e/ a
    %       s[n]=As*[n-1]+u[n]
    3 d7 |, }9 w2 R( g: k/ W6 R%       x[n]=h(s[n])+w[n]
    % x8 ]& X. y% t9 `0 T
    % ?! b4 h! S# h7 O5 v% initialization
    2 p6 n( T" D3 Q) o) O+ U! i- }" Dclear;
    1 f5 ^' q& |% x* S5 ], e" gn=100;$ H) |; f7 n: `; w5 D
    A=[1 0 1 0;0 1 0 1;0 0 1 0;0 0 0 1];9 |6 ^9 p/ z3 z/ e
    rx=zeros(1,n); % 实际位置; Q$ j6 ^. P: ?+ i- O1 M; _
    ry=zeros(1,n); % 实际位置: C* g4 a' Q: P3 u" I' a" b/ t
    vx=zeros(1,n); % 实际速度
    1 D1 r, R1 f) L! e* Dvy=zeros(1,n); % 实际速度
    - P+ r: R* }1 M+ Z* t* H+ ?rxe=zeros(1,n); % 精确位置
    7 e4 T& x/ D' N! Grye=zeros(1,n); % 精确位置
    , P3 X: U& n* {2 K9 G  F/ N* vvxe=zeros(1,n); % 精确速度
    " h- S, [  l& T1 `vye=zeros(1,n); % 精确速度
    # s0 a+ c! V9 e' q" G0 H, c6 ~rx(1,1)=10; % 位置初始& ^2 a* D, ], S8 d
    ry(1,1)=-5; % 位置初始/ X6 J/ N& @/ N0 O" s' q" N# C
    vx(1,1)=-0.2; % 速度初始$ Q# C0 E2 U' O
    vy(1,1)=0.2; % 速度初始$ d" Q. T. q1 w( i
    rxe(1,1)=10;3 G1 \4 d0 r' P; l
    rye(1,1)=-5;
    $ h- V8 Q& R7 F0 nvxe(1,1)=-0.2;
    8 R! W; }: j6 o9 }+ z7 l! b) v& avye(1,1)=0.2;
    ! B( @( B2 L. q- z. Ls=[rx;ry;vx;vy];$ e8 q( H, v; t9 ]7 J
    se=[rxe;rye;vxe;vye];
    # ^6 P! @0 u; @) v  r$ Nr=zeros(1,n);
    + A4 ]) @+ S3 L1 q3 {9 M  lb=zeros(1,n);5 Z- G0 F; z; e! U8 Z- H
    rn=zeros(1,n);, R* E9 ]& H% [( ~" i' l
    re=zeros(1,n);
    ; [& I' }& ]: I+ ybn=zeros(1,n);) S5 [; y( q% o* t$ J
    x=[rn;bn];# ~" l, T( _( U: T/ w5 ^# d
    H=zeros(2,4,n);
    : d" }% |' w/ s& ^' _  Z) k+ x+ v* a: }  x% N
    ux=sqrt(0.00001)*randn(1,n);8 B! h% @1 f; R6 T
    uy=sqrt(0.00001)*randn(1,n);
      z  K9 R' c/ R7 wwr=sqrt(0.001)*randn(1,n);# F8 }9 f$ ]9 C! Q
    wb=sqrt(0.0001)*randn(1,n);' }1 |2 c4 W- \

    % G! |6 n, `7 L5 oQ=[0 0 0 0;0 0 0 0;0 0 cov(ux) 0;0 0 0 cov(uy)]; % 过程噪声矩阵$ }' s1 T- J* k# w/ e
    C=[cov(wr) 0; 0 cov(wb)]; % 观测噪声矩阵
    8 a0 s$ h4 H% J+ Z4 H& Y2 w0 o( l8 f# `1 b2 Z/ ?5 k9 b
    ss=zeros(4,n);% 精确值
    + t0 p- f/ V. n0 E4 msn=zeros(4,n); % 估计修正值
    2 Q4 H+ ~0 Z) t3 k# jsn(:,1)=sn(:,1)+[15;5;0;0]; % 估计修正初始值; ~8 i) v  ~' m" _- B8 s
    ss=zeros(4,n); % 一步预测
    1 e" y, o2 G. whs=zeros(2,n); % 观测值预测
      d$ }1 H+ H* A# y- v4 Y' P/ Dinn=zeros(2,n); % 新息
    : o4 V! b; v; q: V- @M=zeros(4,4,n); % 估计均方误差
    / C5 K: \& r: s' @4 CM(:,:,1)=M(:,:,1)+[1 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1]; % 估计均方误差初始值% a3 Y8 }! N! y8 J/ }& [/ K5 F1 d
    MM=zeros(4,4,n); % 一步预测均方误差
    & ]4 @0 b$ k8 T. O( f% [8 O% [: UK=zeros(4,2,n); % 卡尔曼增益: B8 {5 L" ?; o8 t  R5 G
    % V# W+ T: B0 I% W3 o
    % kalman filtering
      P$ \% S' V9 b0 H4 A, |1 Z: V% N
    # Q0 Y2 ?! h8 x* bfor t=1:n-1& i3 b8 U0 r$ T& R
    %            s[n]=As*[n-1]+u[n]
    4 {) k: ]! @  ?%            x[n]=h(s[n])+w[n]
    . z# ^$ h( L! g0 {' p' R1 M
    + V: i" f+ M$ s- ?  g  f    vx(1,t+1)=vx(1,t)+ux(1,t+1); % 速度过程方程
    7 j* n) z  l, F8 q    vy(1,t+1)=vy(1,t)+uy(1,t+1); % 速度过程方程
    4 d' e0 j# }2 C9 g) X8 w6 |0 r1 N    rx(1,t+1)=rx(1,t)+vx(1,t); % 位置过程方程
    ( K2 C% [' D, w% E  D( X    ry(1,t+1)=ry(1,t)+vy(1,t); % 位置过程方程% h" c& A: k9 w
        s(:,t+1)=[rx(1,t+1);ry(1,t+1);vx(1,t+1);vy(1,t+1)]; % 过程矩阵,维数4*1' s7 h& p5 R- p6 B, H. j
    ; t( {4 J/ ?! N1 c9 U" I
        vxe(1,t+1)=vxe(1,t); % 理论精确值
    " d% X: |" T) W- _    vye(1,t+1)=vye(1,t); % 理论精确值# q' ?4 ], \, j0 H
        rxe(1,t+1)=rxe(1,t)+vxe(1,t); % 理论精确值
    * l8 a2 t  ?% F7 H; j2 `    rye(1,t+1)=rye(1,t)+vye(1,t); % 理论精确值
    / C. c, v' l1 Q7 N% ^8 R, P    se(:,t+1)=[rxe(1,t+1);rye(1,t+1);vxe(1,t+1);vye(1,t+1)]; % 过程矩阵,维数4*1
    0 J/ ^. Z7 x2 f0 K* z& k; Q/ X7 e/ _/ L) m9 y+ v# D8 i1 m
        r(1,t+1)=(rx(1,t+1)^2+ry(1,t+1)^2)^(1/2); % 理论测量距离(含测量误差)
    1 r3 r* r" v' V* C    b(1,t+1)=atan(ry(1,t+1)/rx(1,t+1)); % 理论测量方位(含测量误差)
    + J8 \' G/ u' v, i5 G  Z' `    rn(1,t+1)=r(1,t+1)+wr(1,t+1); % 实际测量距离7 d+ M' z4 V7 C# k8 J- @
        re(1,t+1)=(rxe(1,t+1)^2+rye(1,t+1)^2)^(1/2); % 理论测量距离(含测量误差)
    3 j* F( J) e3 C    bn(1,t+1)=b(1,t+1)+wb(1,t+1); % 实际测量方位
    ! V- \. v3 H( b4 ^    x(:,t+1)=[rn(1,t+1);bn(1,t+1)]; % 观测矩阵,维数2*12 l9 R0 p# t- g! _" t1 K4 k# I) W
    + L! Q3 U2 A, W7 G( D8 e& J
        % 对观测方程求导,得到雅可比矩阵H,维数2*4( t  y4 B! L  ^
        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];0 o- g% v4 V+ X& [
    0 G2 v; m2 z- U3 _* `

    3 k0 R, ~6 {2 B0 c) F    MM(:,:,t+1)=A*M(:,:,t)*A'+Q; % 一步预测均方误差
    # L' x9 I/ ~9 U9 F    K(:,:,t+1)=MM(:,:,t+1)*H(:,:,t+1)'/(C+H(:,:,t+1)*MM(:,:,t+1)*H(:,:,t+1)'); % 卡尔曼增益
    5 `; j6 n5 \2 ]4 N    M(:,:,t+1)=(eye(4)-K(:,:,t+1)*H(:,:,t+1))*MM(:,:,t+1);% 估计均方误差
    4 t  [$ M5 {3 p9 Z, h    ss(:,t+1)=A*sn(:,t); % 一步预测0 m/ E7 N+ l) _2 G" k3 t9 S3 D
        hs(:,t+1)=[((ss(1,t+1))^2+(ss(2,t+1))^2)^(1/2);atan(ss(2,t+1)/ss(1,t+1))]; % 观测值预测  e# s  [7 L7 L' ?) L
        inn(:,t+1)=x(:,t+1)-hs(:,t+1); % 新息5 y* r: y2 X- @- W4 ^7 ?" l8 @
        sn(:,t+1)=ss(:,t+1)+K(:,:,t+1)*inn(:,t+1); % 预测修正# Q+ J0 R, N2 D0 _  x' g
    end" b# S/ p* q  m( D! H; Y3 l
    - E7 v$ k" _1 U! N" ^& r2 b5 q
    subplot(2,1,1);. T1 X) r# l5 }$ x1 \: y0 M
    plot(sn(1,:),sn(2,:)); % kalman估计值# m) Y3 h# y6 t  h- e2 C. o
    hold on;
    / [4 F- P  ~) A- `9 D$ V! zplot(se(1,:),se(2,:),'-r'); % 理论精确值
    + \$ j) }3 Y( H2 |7 M  C2 @7 y
    % G# M+ `% G& G( @subplot(2,1,2); 7 `7 V; Y8 y, Q- H- ~$ {
    plot(rn); % 实际测量距离
    2 @7 ^3 J9 v0 A' T1 Q, B& Ahold on;
    " _/ r: K' u6 [# d1 ?plot(re,'-g'); % 理论精确值
    0 N$ ^5 d& Z% q" t" o  U# Y# G0 F% h
    "真诚赞赏,手留余香"
    还没有人打赏,支持一下
    楼主热帖
    帖文化:【文明发帖 和谐互动】 社区精神:【创新、交流、互助、共享】
  • 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-9-24 20:06

    Powered by Discuz! X3.5 Licensed

    © 2001-2025 Discuz! Team.

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