EDA365电子论坛网

标题: 转：Kalman滤波的Matlab仿真程序解读 [打印本页]
作者: uperrua    时间: 2020-4-27 10:22
标题: 转：Kalman滤波的Matlab仿真程序解读
最近因为要用到Kalman滤波器做东西，所以一直在学习这个东西，鉴于之前的仿真都是用matlab做的，所以呢，kalman滤波器的仿真程序也是用matlab编的。痛苦了几天，几天这个函数终于搞定了，具体的分析如下。

function [zx,zy]=xyKalmanFliter(A,H,Rw,Rv,Rw_c,Rv_c,x0,p0,y）$ ^2 P' z9 g+ d# {* L7 l
%----------------输入参数--------------------------------------------------
0 u( J" t6 g! e! }6 S* d1 k. z%      A -- 系统矩阵
# V) x0 O) J8 P7 w4 a% N* q%      H -- 观察矩阵
9 w# @8 `& @' T0 v* r%     Rw -- 扰动向量0 `% v! N' {/ F
%     Rv -- 测量噪声! X: ~; }/ H( f& O  v
%   Rw_c -- 扰动向量的协方差
* h% L' x; n6 `, d%   Rv_c -- 测量噪声的协方差& n2 [9 \) b8 }% p  _) A
%     x0 -- 节点初始位置向量(x,y)'" F' e/ r1 t  z" i( H
%     p0 -- 初试协方差阵
, S6 I4 q9 n- O- N%      y -- 采样周期; Q* \; y/ N* `( M& T5 r
%--------------------------------------------------------------------------

; B! w4 Z4 K: L7 l
%--------------------------------------------------------------------------/ x+ G7 J* V) G
%   X(k) = A*X(k) + Rw(k)     噪声Q; `! D3 f4 i5 \, a3 t' _
%   Z(k) = H*X(k) + Rv(k)     噪声R
3 T9 f# P; O4 n& U+ `8 C/ \3 M

%  x(k|k-1) = A*x(k-1|k-1) + B*U(k)6 Q9 g! s7 B) B! W; N/ t. j
%  P(k|k-1) = A*P(k-1|k-1)*A' + Q
/ U5 d* A3 v3 j, z0 P9 E9 v%  x(k|k) = x(k|k-1) + kg(k)*(z(k)-H*s(k|k-1))
- s' j9 k4 |6 S* [%  kg(k) = P(k|k-1)*H' / (H*P(k|k-1)*H' + R)
, \( K: n* k$ w( }, a) a) i%  P(k|k) = (I-kg(k)*H)*P(k|k-1)1 i* q5 c0 F$ I
%--------------------------------------------------------------------------

len = length( y );                                                        %获取采样点数

r = size(A,1);                                                            %获取系统矩阵A的行数" E3 h3 k6 U, M5 a: p  o
I= eye( r(1) );                                                           %生成单位矩阵3 o# s- E- A" U5 ]4 D$ c7 y8 @
P1 = zeros( r(1),r(1) );                                                  %初始化协方差阵

%初始化节点位置，协方差阵
5 o+ s% R7 O3 t8 ]9 X' K/ V0 b X = x0;. l) {0 ]6 k* V; s7 O
P = p0;

len1 = size( H*X ,1);
& L- }6 s& s# i1 Z- p for s=1:1:len
" ^% c: A8 {- f1 S' y     z1 = A*X+ Rw;                        % X(k) = A*X(k) + Rw(k)     协方差  Rw_c
8 r( {& v6 F2 Y7 T3 [     zx(1:r(1),s) = z1(1:r(1)) ;   
% D3 h% ^, z7 q     z2 = H*X + Rv;                       % Z(k) = H*X(k) + Rv(k)     协方差  Rv_c4 P' }* q5 [9 H  c( O( i  X& A1 e
     zy(1:len1,s) = z2(1:len1 );0 m) g* G& g3 V* e2 |3 i
    ) w+ |: r, j  n/ K8 V" u% o* G5 k5 f
     P1 = A*P*A' + Rw_c;                  %  P(k|k-1) = A*P(k-1|k-1)*A' + Q
1 B* E1 @, U/ [7 i     K = P1*H'*inv( H*P1*H' + Rv_c );     %  kg(k) = P(k|k-1)*H' / (H*P(k|k-1)*H' + R)) g% y5 F* |8 G- [* w
     X = A*X + K*( zy(1:len1,s) - H*A*X );      

                                          %  x(k|k) = x(k|k-1) + kg(k)*(z(k)-H*s(k|k-1))# s- s+ N+ x0 f
     P = ( I - K*H ) * P1;                %  P(k|k) = (I-kg(k)*H)*P(k|k-1)
- Z1 X/ K0 j! `9 z# d2 T" [ end

, Y" E" Q0 X  W, \; R8 H
return

原文地址：转：Kalman滤波的Matlab仿真程序解读作者：浩瀚

' a; S- `. U7 ^: d/ A- T
作者: CCxiaom    时间: 2020-4-27 13:27
Kalman滤波的Matlab仿真程序解读



欢迎光临 EDA365电子论坛网 (https://bbs.eda365.com/) Powered by Discuz! X3.2