找回密码
 注册
关于网站域名变更的通知
查看: 512|回复: 1
打印 上一主题 下一主题

卡尔曼滤波基本方程

[复制链接]
  • TA的每日心情
    难过
    2019-11-19 16:03
  • 签到天数: 1 天

    [LV.1]初来乍到

    跳转到指定楼层
    1#
    发表于 2020-1-15 13:51 | 只看该作者 回帖奖励 |倒序浏览 |阅读模式

    EDA365欢迎您登录!

    您需要 登录 才可以下载或查看,没有帐号?注册

    x
    %Title卡尔曼滤波基本方程
    ( f( u6 D7 K# y# n: \. H%A1初始化
    ) t$ i! T+ u. p  c1 x7 P
      ~" A' X$ n. A& m7 e. d! I. |$ u%A1.1参数初始化1 u/ q4 Y3 l# t; h) Q4 h9 P
    N=200;
    $ W+ W' f) v) n  M
    0 j" D3 j* Q( XXa=zeros(N,1);%预估值
    ) s% g. \$ p0 qX=zeros(N,1);%当前时刻最优估计值1 z; }1 v/ n. T9 k/ e! U" g
    Pa=zeros(N,1);%Xa的方差矩阵5 b: G+ E0 i+ Z. x  V5 v1 T: |
    P=zeros(N,1);%X的方差矩阵
    6 x$ F$ T' |; e1 |' B2 _Kg=ones(N,1);%卡尔曼增益! F& i, o, {4 q2 A5 I% s
    # S7 s+ e' o; g2 z9 M. j7 P8 S; u
    Z=zeros(N,1);%观测值. L" f" o3 l- J9 l0 w3 C. {2 O3 ~5 L* I
    8 f) [1 D8 i) z! P3 Y6 e/ h, K( M
    W=randn(N,1);%过程噪声
    ( o1 Y( [7 [2 e$ z. d, JstdW=std(W);%标准差
    $ ^4 a6 Q6 C% b1 u7 F6 Y) g$ B, |varW=stdW.^2;%方差( \. G( Y8 b4 W! H" P
    V=0.5*randn(N,1);%观测噪声; H- O9 x; c! ?4 w7 o
    stdV=std(V);%标准差
    ' c6 v7 A% u( |) Q, O  q& |$ }varV=stdV.^2;%方差
    9 ^% O/ a8 |2 k2 H! c+ P  @. ?: ]1 U9 N0 K# C
    A=1;
    ; m  `' X+ N4 s3 C- d* aH=1;
    , I' N, O& y( c( a$ K; m  J( W; r, u8 B/ O
    %A1.2状态初始化$ t5 D% E& [3 A+ h( O
    X(1)=23;%初始估计温度7 R: }" P: e3 Z2 h) I& F1 E
    P(1)=10;%初始估计方差
    & D6 O$ Z3 _! UZ(1)=25+V(1);
    % r( Z6 p) E2 f* O5 wXa(1)=23+W(1);
    ) P; B. E, @$ |1 v) P5 F2 I, s$ K) p3 |4 l
    %卡尔曼滤波
    6 W2 r9 D' n, e% a" R4 @# _for k=2:N7 [( x7 A' i3 v  e+ ]- p* h
        %预估
    3 c) K* Y& a  b7 h, W& f    Xa(k)=A*X(k-1);# C9 X. k9 T4 X! P/ {
        Pa(k)=A*P(k-1)*A'+varW;
    : v' B) _' V) D+ Y0 Y6 m9 X/ e$ u% b' h5 C
        %校正3 R' \9 A% ]" U% _, `: ~: n" o
        Kg(k)=Pa(k)*H'/(H*Pa(k)*H'+varV);* t9 N5 y* i) O
        Z(k)=25+V(k);%25是真实温度值8 u; O, I! E2 f; M
        X(k)=Xa(k)+Kg(k)*(Z(k)-H*Xa(k));. W0 q. q7 a. M) |" z
        P(k)=(1-Kg(k)*H)*Pa(k);
    + L' r7 G/ x8 m2 c3 _end& E! ]3 E! x" @

    9 e4 O+ I( g; ?+ j6 a# Xt=1:N;3 k' D$ g- K; Y5 b0 Q/ U' S
    realVal=25*ones(N,1);
    $ ~% \" ]7 y9 R9 T* z# w8 a4 z! n: c& U$ R2 a) G: F: ]+ O+ _
    plot(t,realVal,'y',t,Z,'c',t,Xa,'b',t,X,'r');; y) N2 f, y; ]' o" K% ^. k
    axis([0 N 20 30]);  M/ u. K7 J. f) d
    legend('真实值','观测值','预估值','估计值');/ @" @, j% Z) ~6 i+ f' v. X
    grid on;
    1 D( |8 f: Z. U- i* j6 q5 y8 }' g( H+ Q6 d+ m( m1 G, s
    title('Kalman Filter');
    ' H, _0 Y2 k. t# u! ~! Y
    & N. N- R; f; h/ o; j" A: ]2 |, `%realVal真实值;Z观测值;Xa预估值;X最优估计值;W过程噪声;V观测噪声
    % o, M3 G2 S7 h' H# E% q3 D) s7 h9 ~- K
    您需要登录后才可以回帖 登录 | 注册

    本版积分规则

    关闭

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

    EDA365公众号

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

    GMT+8, 2025-10-30 17:15 , Processed in 0.125000 second(s), 23 queries , Gzip On.

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

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

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