QQ登录

只需要一步,快速开始

 注册地址  找回密码
查看: 4637|回复: 6
打印 上一主题 下一主题

[问题求助] 卡尔曼算法的matlab程序

[复制链接]
字体大小: 正常 放大
工科男 实名认证       

13

主题

3

听众

473

积分

升级  57.67%

  • TA的每日心情
    开心
    2014-7-4 12:51
  • 签到天数: 41 天

    [LV.5]常住居民I

    发帖功臣

    跳转到指定楼层
    1#
    发表于 2011-11-26 16:16 |只看该作者 |倒序浏览
    |招呼Ta 关注Ta
    求卡尔曼算法的matlab预测程序
    zan
    转播转播0 分享淘帖0 分享分享0 收藏收藏0 支持支持0 反对反对0 微信微信

    1341

    主题

    738

    听众

    2万

    积分

    数学中国总编辑

  • TA的每日心情

    2016-11-18 10:46
  • 签到天数: 206 天

    [LV.7]常住居民III

    超级版主

    社区QQ达人 邮箱绑定达人 元老勋章 发帖功臣 新人进步奖 原创写作奖 最具活力勋章 风雨历程奖

    群组2011年第一期数学建模

    群组第一期sas基础实训课堂

    群组第二届数模基础实训

    群组2012第二期MCM/ICM优秀

    群组MCM优秀论文解析专题

    matlab下面的kalman滤波程序' @3 A4 ~2 k- U) k1 |+ O! I9 s
    clear  N=200; w(1)=0;   
    3 b7 N* q, b; J. Vw=randn(1,N)
    1 t3 }% V/ C: ]( r/ Qx(1)=0;   $ @# {2 A& W+ Q
    a=1;   
    6 r" J% ?% `; f$ Xfor k=2:N;   
    . [2 g9 g1 P0 S: a& J6 k2 E1 vx(k)=a*x(k-1)+w(k-1);   % |( o; y: S+ J$ i) T
    end   ( r* x, b* z  q
    V=randn(1,N);   ) t) ], |2 j! R# C
    q1=std(V);   
    0 E7 Z: d* B, z% M0 ~/ _9 JRvv=q1.^2;   
    ) j; q7 U% B2 Iq2=std(x);   / p5 j) D* X  _( e+ a% w& H
    Rxx=q2.^2;   
    ( d. f7 W1 a# x0 f' xq3=std(w);   ! I1 \$ t7 J1 h3 T* I
    Rww=q3.^2;   
    $ Q( K) x) {3 ?- K2 ^" Xc=0.2;   ) }2 j; E4 Q8 I5 n
    Y=c*x+V;   * m! S$ M' l" }" p: B7 [
    p(1)=0;   
    3 Y' U  G0 _  C" A& ~; O& vs(1)=0;* z% A1 r$ r- q1 f
    for t=2:N;   
    8 _# p6 d+ H0 |$ B/ d; D! mp1(t)=a.^2*p(t-1)+Rww;   
    0 j6 s  i. f' \  Z" f) ?* J) F/ wb(t)=c*p1(t)/(c.^2*p1(t)+Rvv);   5 O9 U: z3 {. I2 l
    s(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1));   $ A- H  f) k$ q0 F
    p(t)=p1(t)-c*b(t)*p1(t);   
      L* ]9 T; s3 mend   - T3 C  j, o: f9 i6 [, F* x
    t=1:N;   
    * @4 ]/ H2 o" H9 C. |2 m7 Wplot(t,s,'r',t,Y,'g',t,x,'b');   . Q: ]. h4 d7 M+ }! U
    function [x, V, VV, loglik] = kalman_filter(y, A, C, Q, R, init_x, init_V, varargin)   
    , P; V* Q& C) y3 z5 i. r' U, v% Kalman filter.   
    % ]  X! M) {# r) o) |% [x, V, VV, loglik] = kalman_filter(y, A, C, Q, R, init_x, init_V, ...)   
    ; \0 n* ~1 m, U! j  m( P0 X%   
    + }  Z. s+ W, F) A* ?/ R! S% INPUTS:   2 e1 ~' e6 g1 y; {4 I; }0 f
    % y(:,t) - the observation at time t   & a3 J5 Q- v) d5 y( M& D( n
    % A - the system matrix   
    / a, x2 C) f5 k: `: [% C - the observation matrix   
    % r/ Y% W; ~/ F* H" T) G% Q - the system covariance   # ?4 J" r& t1 M
    % R - the observation covariance   
    ' M- F2 R+ I, S9 Q7 l$ u2 E% init_x - the initial state (column) vector   4 ]1 _4 G# i& [* O' z- n
    % init_V - the initial state covariance   9 @  H' y: x& x
    %   
    ! z9 i% q  ?& _+ b1 I! n( j; K% OPTIONAL INPUTS (string/value pairs [default in brackets])   
    . D7 j3 v, \2 I# j' `" D* M% 'model' - model(t)=m means use params from model m at time t [ones(1,T) ]   
    / h/ ~, G: @" P0 \9 D: Z- Z% In this case, all the above matrices take an additional final dimension,   # J% q8 _5 c  z' v
    % i.e., A(:,:,m), C(:,:,m), Q(:,:,m), R(:,:,m).   
    . e% ^$ v! ?. @4 F- X4 b- z( j) s% However, init_x and init_V are independent of model(1).   ! J. I" e, ]. x, A$ M' ~) s
    % 'u' - u(:,t) the control signal at time t [ [] ]   ; G* {9 z2 F2 O2 ^4 c  ?5 z( ^
    % 'B' - B(:,:,m) the input regression matrix for model m   - i  h0 }3 A' k7 i5 x6 ]
    %   * s9 Q% t# m. Q# D  \( ~- H" c" u4 U
    % OUTPUTS (where X is the hidden state being estimated)   : S& w. O: u* j# L7 W
    % x(:,t) = E[X(:,t) | y(:,1:t)]   7 |  r, f' p. Z, F. U2 B0 B) w
    % V(:,:,t) = Cov[X(:,t) | y(:,1:t)]   
      a- I$ \' g' T# X: t* y7 O% VV(:,:,t) = Cov[X(:,t), X(:,t-1) | y(:,1:t)] t >= 2   0 u6 |7 m- q. o. S" O
    % loglik = sum{t=1}^T log P(y(:,t))   - {8 J( F4 X( h! M5 n1 ?$ D
    %   2 p" K# H8 s7 \( z) u
    % If an input signal is specified, we also condition on it:   # T9 I; y$ b. N
    % e.g., x(:,t) = E[X(:,t) | y(:,1:t), u(:, 1:t)]   % \+ S+ P. F' C9 R
    % If a model sequence is specified, we also condition on it:   ; G- k( ^. `, {# o1 J
    % e.g., x(:,t) = E[X(:,t) | y(:,1:t), u(:, 1:t), m(1:t)]   
    ) L7 H% z( e# d# D1 r$ X; a[os T] = size(y);   - e5 |0 C0 C( M) t" G% ^
    ss = size(A,1); % size of state space   
    ' S- v' O* U2 L( N- Y, ]; H& v3 ~" |% set default params   
    & {" a+ v8 Y; S: B* Pmodel = ones(1,T);   
    ; D7 X0 `; @. n' Zu = [];   : {2 H  I1 v8 B  P
    B = [];   / z2 ^( m, E7 Y) h( A; ~
    ndx = [];   
    ! S. ~, `! T. m- Gargs = varargin;   
    * X$ b' q& ^; ~$ n: Knargs = length(args);   % o. M( j8 q5 @& Y2 n" Z1 C
    for i=1:2:nargs   
    & ^- n. \! V  ~switch args   
    / p8 |& w; r1 y3 Zcase 'model', model = args{i+1};   0 N/ G' X4 M' P* P
    case 'u', u = args{i+1};   
    ; C. `& J# r) b5 ocase 'B', B = args{i+1};     l$ u) @1 [: O1 A6 m  x
    case 'ndx', ndx = args{i+1};   5 n: L+ t/ X% ]$ R2 b. L8 ?
    otherwise, error(['unrecognized argument ' args])   $ h+ r, T# P) X  C6 E6 r4 E' k
    end   $ W3 q, ~3 L- Y9 L6 i& h/ J, X
    end   # C, d8 P; \! h/ c
    x = zeros(ss, T);   1 |0 U6 t' y8 {# @% ~% d
    V = zeros(ss, ss, T);   : a& a9 M& B. ]! _
    VV = zeros(ss, ss, T);   
    . c- W( @  G+ \9 \3 ]8 \loglik = 0;   0 q0 l: Z1 f0 b; H- F  V$ L
    for t=1:T   m = model(t);   
    ) E; t! V0 H2 n. O' Bif t==1   %prevx = init_x(:,m);   
    0 N9 J# n& m  M; `) }7 L, ^) n. }%prevV = init_V(:,:,m);   
    % Y7 {% {6 v7 `5 [% P( zprevx = init_x;   
    ; f- Q+ s$ o' o$ G3 G+ ?prevV = init_V;   
    " W9 p& ^$ [8 E6 t( {+ }initial = 1;   
    + s7 D8 [0 u& a. s% M& r+ Yelse   prevx = x(:,t-1);   / W4 C, O( {* |
    prevV = V(:,:,t-1);   
    # {6 V  @" ?) q0 cinitial = 0;   7 n- q  F7 _; [/ L5 N. h* U6 @
    end   7 X7 Y4 G/ Q& z/ v. m/ c
    if isempty(u)   
    ! U( [+ ?8 A) _& y[x(:,t), V(:,:,t), LL, VV(:,:,t)] = ...   
    0 T' w, s, i; A' ]kalman_update(A(:,:,m), C(:,:,m), Q(:,:,m), R(:,:,m), y(:,t), prevx, prevV, 'initial', initial);   else   . ]4 G4 i8 z7 P$ b
      if isempty(ndx)   [x(:,t), V(:,:,t), LL, VV(:,:,t)] = ...   
    8 w6 F% o1 B" G! n" J, P     kalman_update(A(:,:,m), C(:,:,m), Q(:,:,m), R(:,:,m), y(:,t), prevx, prevV, ...   'initial', initial,     'u', u(:,t), 'B', B(:,:,m));   " U6 b- I  z1 E1 r
    else   
    4 {/ y0 O5 G* w1 H: a% t0 Y2 pi = ndx;   ) h1 d) l/ H. V! [! w
    % copy over all elements; only some will get updated   x(:,t) = prevx;   . A! a( [/ X- V# M% O
    prevP = inv(prevV);   
    4 }( B  t2 h$ d: j9 I* p; G* iprevPsmall = prevP(i,i);   
    * d; q6 L5 I$ l5 y) C" P2 LprevVsmall = inv(prevPsmall);   4 b& O, K/ f7 n; e7 |7 z3 g
    [x(i,t), smallV, LL, VV(i,i,t)] = ...   kalman_update(A(i,i,m), C(:,i,m), Q(i,i,m), R(:,:,m), y(:,t), prevx(i), prevVsmall, ...   'initial', initial, 'u', u(:,t), 'B', B(i,:,m));   
    1 J: E. @) V" D6 V* MsmallP = inv(smallV);   6 O; Y: B* S' _
    prevP(i,i) = smallP;   ( `$ U( q4 N4 E3 b" ^& g
    V(:,:,t) = inv(prevP);   % s$ A) Y. h0 s- t  b" q
    end   
    8 w4 `0 n$ ]1 s6 [3 ?  Y7 E9 F3 kend   & n1 w( Z$ A! n8 R; I1 b; J4 Y
    loglik = loglik + LL;   
    3 M: a' d" d8 L7 W. m" jend
    回复

    使用道具 举报

    1

    主题

    3

    听众

    349

    积分

    升级  16.33%

  • TA的每日心情
    奋斗
    2014-7-16 18:27
  • 签到天数: 123 天

    [LV.7]常住居民III

    社区QQ达人

    群组2012第三期美赛培训

    厚积薄发 发表于 2011-11-28 10:48
    # o/ H' G7 I+ |1 ^matlab下面的kalman滤波程序3 H, a- ]$ Q  {  X3 Q5 A9 F/ i
    clear  N=200; w(1)=0;   + i, A/ b% o5 i, M; \
    w=randn(1,N)

    . s4 g7 f. `1 q0 D, S强大呀,下了
    回复

    使用道具 举报

    工科男 实名认证       

    13

    主题

    3

    听众

    473

    积分

    升级  57.67%

  • TA的每日心情
    开心
    2014-7-4 12:51
  • 签到天数: 41 天

    [LV.5]常住居民I

    发帖功臣

    回复

    使用道具 举报

    244190977        

    1

    主题

    8

    听众

    80

    积分

    升级  78.95%

  • TA的每日心情
    开心
    2014-11-13 19:31
  • 签到天数: 44 天

    [LV.5]常住居民I

    自我介绍
    研究生

    社区QQ达人

    回复

    使用道具 举报

    yulun9988        

    3

    主题

    11

    听众

    524

    积分

    升级  74.67%

  • TA的每日心情
    擦汗
    2015-2-12 23:58
  • 签到天数: 108 天

    [LV.6]常住居民II

    自我介绍
    运用遗传算法

    邮箱绑定达人

    群组Matlab讨论组

    回复

    使用道具 举报

    yulun9988        

    3

    主题

    11

    听众

    524

    积分

    升级  74.67%

  • TA的每日心情
    擦汗
    2015-2-12 23:58
  • 签到天数: 108 天

    [LV.6]常住居民II

    自我介绍
    运用遗传算法

    邮箱绑定达人

    群组Matlab讨论组

    回复

    使用道具 举报

    您需要登录后才可以回帖 登录 | 注册地址

    qq
    收缩
    • 电话咨询

    • 04714969085
    fastpost

    关于我们| 联系我们| 诚征英才| 对外合作| 产品服务| QQ

    手机版|Archiver| |繁體中文 手机客户端  

    蒙公网安备 15010502000194号

    Powered by Discuz! X2.5   © 2001-2013 数学建模网-数学中国 ( 蒙ICP备14002410号-3 蒙BBS备-0002号 )     论坛法律顾问:王兆丰

    GMT+8, 2025-8-1 05:02 , Processed in 0.557402 second(s), 83 queries .

    回顶部