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

 找回密码
 立即加入
搜索
查看: 634|回复: 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( ~/ L* d  ]0 z0 \( k7 K
    % vx[n]=vx[n-1]+ux[n]& B$ u/ K, c# f( {5 ^- E
    % vy[n]=vy[n-1]+uy[n]
    1 R+ p! i4 n4 {- `0 x% rx[n]=rx[n-1]+vx[n-1]*t! t1 g. R# v' N, C" M( c2 v$ t
    % ry[n]=ry[n-1]+vy[n-1]*t
    . M4 y' U2 i" ]  N7 q% s[n]=[rx[n];ry[n];vx[n];vy[n]]; V" M* Q, U$ o# E6 c  }
    % A=[1 0 t 0;0 1 0 t;0 0 1 0;0 0 0 1]4 B: U6 L; J9 p7 I8 p( D7 \$ O
    % s[n-1]=[rx[n-1];ry[n-1];vx[n-1];vy[n-1]], J( E# d2 H1 {, A9 w. _* N+ T
    % u[n]=[0;0;ux[n];uy[n]]' i  ~( m, ]2 m. c. Q: @
    %       s[n]=As*[n-1]+u[n]
    ) C5 z. o; s2 d( B; }4 S: M- m%       x[n]=h(s[n])+w[n]8 e5 g4 w$ O- x8 A9 e1 [

    " h# c; q- i) d- ~% initialization; m) o+ T1 \( `7 G# p4 s
    clear;: v, ?: Q* A9 u( ]! {
    n=100;
      M1 a2 @% [3 I7 L! D& ^" yA=[1 0 1 0;0 1 0 1;0 0 1 0;0 0 0 1];
    ( @1 Y3 q: {6 Q! j% ]& xrx=zeros(1,n); % 实际位置
    : `& k4 X6 w6 @" d, ury=zeros(1,n); % 实际位置
    % U& p' D. L' I+ }% M- |1 Z( V" a$ Avx=zeros(1,n); % 实际速度! P- p; q+ ]! @
    vy=zeros(1,n); % 实际速度
    & x/ C5 X/ a/ f* D4 ~rxe=zeros(1,n); % 精确位置
    9 ^& m( y2 h- `. r9 H5 r" prye=zeros(1,n); % 精确位置
    7 o4 T+ A" o; u& yvxe=zeros(1,n); % 精确速度) c# j9 ~) ?/ K% O. }+ X9 B1 G
    vye=zeros(1,n); % 精确速度/ B3 O- T! s9 x5 {
    rx(1,1)=10; % 位置初始
    2 C! P& B$ H  O* b# S- b, S3 ary(1,1)=-5; % 位置初始
    ) m% I! C: B, f, Z0 yvx(1,1)=-0.2; % 速度初始% ]" Z4 c9 z* S0 j
    vy(1,1)=0.2; % 速度初始% u0 B: ~# c3 K
    rxe(1,1)=10;9 C6 U, |! w0 r% k% G& T7 K% L0 y1 E
    rye(1,1)=-5;' N. s; ~( C- x3 H
    vxe(1,1)=-0.2;
    # ^  S+ E2 n0 q9 h- r0 Bvye(1,1)=0.2;
    - d+ m2 u& [7 y- o) Ws=[rx;ry;vx;vy];& k2 r- w% J0 u4 }+ H
    se=[rxe;rye;vxe;vye];2 a+ v- q( x* X) p  \
    r=zeros(1,n);4 ?  q; `8 v; C' e4 g/ Z1 I
    b=zeros(1,n);0 c8 [  G! U0 `* o/ U# \0 ?9 Z
    rn=zeros(1,n);4 d3 ?& Z, ~. N6 Z  c, U, m
    re=zeros(1,n);
    - P- s3 a1 }0 Y" v  s- ?4 ~2 `bn=zeros(1,n);
    2 M% W5 a. k5 _# s4 ~3 d) M: b  ix=[rn;bn];7 Q  S* v7 j0 G7 @. L
    H=zeros(2,4,n);
    ; Y) F; Y; G& X) W, F0 h( }, T( @
    ( R- d/ L2 v: B6 ?ux=sqrt(0.00001)*randn(1,n);
      q, R) @7 P  ^1 G7 \& {uy=sqrt(0.00001)*randn(1,n);( ~3 w: R, H, C0 ]
    wr=sqrt(0.001)*randn(1,n);
    2 C4 N5 M! {, ]4 Y. Uwb=sqrt(0.0001)*randn(1,n);
    / i3 K, G: F9 r% t& j! M
    ( t! C1 e5 F8 ~  d! b3 JQ=[0 0 0 0;0 0 0 0;0 0 cov(ux) 0;0 0 0 cov(uy)]; % 过程噪声矩阵
    1 V, `$ h) x" ]. u% ZC=[cov(wr) 0; 0 cov(wb)]; % 观测噪声矩阵$ L+ _0 T* b, Y- a7 K8 {5 p

    % N! {% [, D3 u: Z% m8 Bss=zeros(4,n);% 精确值2 a0 E$ b1 ]' c+ s# L
    sn=zeros(4,n); % 估计修正值$ X) |7 T4 F" j) d% k+ ?6 b# O
    sn(:,1)=sn(:,1)+[15;5;0;0]; % 估计修正初始值7 A+ Q' X1 t' b. j1 w
    ss=zeros(4,n); % 一步预测
    6 q+ F3 Y4 i7 |* X! ohs=zeros(2,n); % 观测值预测
    5 U2 [$ t3 c( F+ p6 e/ iinn=zeros(2,n); % 新息4 z( P' ]2 q8 P: {* u% m
    M=zeros(4,4,n); % 估计均方误差
    7 W2 U: |. z3 `# {M(:,:,1)=M(:,:,1)+[1 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1]; % 估计均方误差初始值1 S9 \& ~7 m1 {5 O1 F$ |7 t
    MM=zeros(4,4,n); % 一步预测均方误差
    % v7 R# t0 b( F8 xK=zeros(4,2,n); % 卡尔曼增益% o  D" U; {2 r
    * K4 W. X6 M* T
    % kalman filtering3 v' |/ f% Z- v4 m" }% e- J
    " S2 ?, {; U" A7 f' W, K! W/ C
    for t=1:n-1
    8 c/ H; L9 P9 j( r7 ^% G0 e3 s%            s[n]=As*[n-1]+u[n]
    : L' T6 u1 @' R; o%            x[n]=h(s[n])+w[n]
    . ]% q- Q/ G5 E. f( m% D8 d# Y) h9 v$ B6 n% e! e5 D9 E
        vx(1,t+1)=vx(1,t)+ux(1,t+1); % 速度过程方程& C0 K+ z$ ?1 u0 E2 @- W
        vy(1,t+1)=vy(1,t)+uy(1,t+1); % 速度过程方程
    ' C/ `. P0 Y; B    rx(1,t+1)=rx(1,t)+vx(1,t); % 位置过程方程
    * X5 y! w5 A. _    ry(1,t+1)=ry(1,t)+vy(1,t); % 位置过程方程
    3 Q0 S" z. @+ V* [: l- f    s(:,t+1)=[rx(1,t+1);ry(1,t+1);vx(1,t+1);vy(1,t+1)]; % 过程矩阵,维数4*10 g2 p9 u6 x% A) M( v6 t/ C
    1 D2 e3 V7 v8 [. ^  @
        vxe(1,t+1)=vxe(1,t); % 理论精确值. q. I+ w2 e) d# m$ M: H. @3 t
        vye(1,t+1)=vye(1,t); % 理论精确值( H% D& Q6 `0 a0 D6 f3 B6 G
        rxe(1,t+1)=rxe(1,t)+vxe(1,t); % 理论精确值
    # `# C! \- U+ h4 ^+ ^! `- j% H    rye(1,t+1)=rye(1,t)+vye(1,t); % 理论精确值7 v5 v. w& Y# f2 t- E
        se(:,t+1)=[rxe(1,t+1);rye(1,t+1);vxe(1,t+1);vye(1,t+1)]; % 过程矩阵,维数4*1' V1 C% J5 o- j7 q3 q6 D

    7 h$ }; B& v* u3 H! n, K    r(1,t+1)=(rx(1,t+1)^2+ry(1,t+1)^2)^(1/2); % 理论测量距离(含测量误差), h8 N' Z1 c4 Y, V6 j4 y
        b(1,t+1)=atan(ry(1,t+1)/rx(1,t+1)); % 理论测量方位(含测量误差)
    6 [* o2 p' T& C2 t& N    rn(1,t+1)=r(1,t+1)+wr(1,t+1); % 实际测量距离2 K% H1 Z/ q3 f( Z4 B  i, y
        re(1,t+1)=(rxe(1,t+1)^2+rye(1,t+1)^2)^(1/2); % 理论测量距离(含测量误差)0 l( H4 a# D# Y! t0 h
        bn(1,t+1)=b(1,t+1)+wb(1,t+1); % 实际测量方位
    0 t0 ^, U. }% f9 F( E  v$ c    x(:,t+1)=[rn(1,t+1);bn(1,t+1)]; % 观测矩阵,维数2*1
    : V$ R% L$ k/ i* a$ g$ N) @/ Q0 R
        % 对观测方程求导,得到雅可比矩阵H,维数2*4
    + D) t' a1 k; R1 c& E    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];, y; U: S6 p, V: A) A- d6 P

    2 O; N: O! D2 f  A# B$ z; X& e; l/ L" y. Y! C
        MM(:,:,t+1)=A*M(:,:,t)*A'+Q; % 一步预测均方误差6 |6 f: V3 D& v. O6 }( |" w: Q
        K(:,:,t+1)=MM(:,:,t+1)*H(:,:,t+1)'/(C+H(:,:,t+1)*MM(:,:,t+1)*H(:,:,t+1)'); % 卡尔曼增益$ \% P0 X3 e2 ^
        M(:,:,t+1)=(eye(4)-K(:,:,t+1)*H(:,:,t+1))*MM(:,:,t+1);% 估计均方误差7 b" c# l/ J6 K* R/ r' n3 z2 |6 W. G
        ss(:,t+1)=A*sn(:,t); % 一步预测' \. t; v) u7 X: @5 F+ h) J5 ^
        hs(:,t+1)=[((ss(1,t+1))^2+(ss(2,t+1))^2)^(1/2);atan(ss(2,t+1)/ss(1,t+1))]; % 观测值预测
    ) s9 N( `  v, |    inn(:,t+1)=x(:,t+1)-hs(:,t+1); % 新息
    0 |2 k5 h; H- R    sn(:,t+1)=ss(:,t+1)+K(:,:,t+1)*inn(:,t+1); % 预测修正* X- M! t& Z7 H# w8 t
    end
    8 c6 p" D1 t& W' \. a& |/ w( f2 g( X9 ^# `1 S. m, w/ k
    subplot(2,1,1);  Z- Q/ U, \/ E
    plot(sn(1,:),sn(2,:)); % kalman估计值
    & Z* B% _3 ?6 `, ?/ a% d7 F0 whold on;
    / Y& n- ?  G1 S, J! eplot(se(1,:),se(2,:),'-r'); % 理论精确值
    * X6 U: ~: d* J
    / L+ F9 T+ g8 j( Tsubplot(2,1,2); 3 M- r, o8 S* y# p  N
    plot(rn); % 实际测量距离
    0 {- {- F/ I. ~9 G( lhold on;7 ^( G/ ]* o0 W- w* q% s9 g
    plot(re,'-g'); % 理论精确值+ U% |- n& G% g1 G  V
    & M; R7 ~1 e5 w
    "真诚赞赏,手留余香"
    还没有人打赏,支持一下
    楼主热帖
    帖文化:【文明发帖 和谐互动】 社区精神:【创新、交流、互助、共享】
  • 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-4 03:21

    Powered by Discuz! X3.5 Licensed

    © 2001-2025 Discuz! Team.

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