扩展卡尔曼滤波小程序

HYSYYRPS

% function  EXT Kalman Filtering
% vx[n]=vx[n-1]+ux[n]
8 J# D. i& e5 n( H% vy[n]=vy[n-1]+uy[n]
% rx[n]=rx[n-1]+vx[n-1]*t
% ry[n]=ry[n-1]+vy[n-1]*t
+ f+ _* |2 i* c0 ]% M: ?5 D% s[n]=[rx[n];ry[n];vx[n];vy[n]]
% A=[1 0 t 0;0 1 0 t;0 0 1 0;0 0 0 1]
% s[n-1]=[rx[n-1];ry[n-1];vx[n-1];vy[n-1]]
% u[n]=[0;0;ux[n];uy[n]]
9 ?2 n) H5 @' X; \  E4 z( ^0 R* s7 `$ b%       s[n]=As*[n-1]+u[n]
%       x[n]=h(s[n])+w[n]

% initialization
clear;
* y4 I% ?( a2 w8 R0 a7 kn=100;
A=[1 0 1 0;0 1 0 1;0 0 1 0;0 0 0 1];
rx=zeros(1,n); % 实际位置
ry=zeros(1,n); % 实际位置
( `2 k4 O1 n8 m2 ^1 t% c0 b# \vx=zeros(1,n); % 实际速度
vy=zeros(1,n); % 实际速度
rxe=zeros(1,n); % 精确位置
rye=zeros(1,n); % 精确位置
1 S. k& O1 d) R! _$ @, Kvxe=zeros(1,n); % 精确速度
vye=zeros(1,n); % 精确速度
rx(1,1)=10; % 位置初始
5 ?' l( ^4 S2 Z" n. ~/ rry(1,1)=-5; % 位置初始
vx(1,1)=-0.2; % 速度初始
vy(1,1)=0.2; % 速度初始
+ Q5 ]4 Q% e  N4 i9 s& r4 prxe(1,1)=10;
rye(1,1)=-5;
vxe(1,1)=-0.2;
% s/ @* Y9 i% F5 Y1 d# Uvye(1,1)=0.2;
s=[rx;ry;vx;vy];
se=[rxe;rye;vxe;vye];
3 Z! H9 j* ?. O2 J& H# lr=zeros(1,n);
b=zeros(1,n);
- P, W- ?  c; g6 ]3 x  \: rrn=zeros(1,n);
2 H" L% v2 s+ B. @6 _re=zeros(1,n);
# j. P; M" A* a3 _9 L; pbn=zeros(1,n);
* @  ~5 \2 G1 M! y/ x6 [x=[rn;bn];
H=zeros(2,4,n);

/ ]+ E7 s8 F! c/ _/ b3 Zux=sqrt(0.00001)*randn(1,n);
uy=sqrt(0.00001)*randn(1,n);
wr=sqrt(0.001)*randn(1,n);
6 y  T8 z& a6 `) y& O( V  K6 {wb=sqrt(0.0001)*randn(1,n);

- j0 ~) [; F5 [) H  o( EQ=[0 0 0 0;0 0 0 0;0 0 cov(ux) 0;0 0 0 cov(uy)]; % 过程噪声矩阵
$ y+ }# t4 A7 J6 V" G+ G+ `$ I) |C=[cov(wr) 0; 0 cov(wb)]; % 观测噪声矩阵
) o- `, T' j2 l- C" ?; s' z- ]4 W
ss=zeros(4,n);% 精确值
& H+ E1 Z, K+ g! @sn=zeros(4,n); % 估计修正值
) D- |2 S* g4 R6 p/ l% E0 dsn(:,1)=sn(:,1)+[15;5;0;0]; % 估计修正初始值
ss=zeros(4,n); % 一步预测
hs=zeros(2,n); % 观测值预测
9 y. k: }5 t7 |0 k* `4 U8 pinn=zeros(2,n); % 新息
M=zeros(4,4,n); % 估计均方误差
M(:,:,1)=M(:,:,1)+[1 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1]; % 估计均方误差初始值
0 l, L1 t1 U9 E6 hMM=zeros(4,4,n); % 一步预测均方误差
K=zeros(4,2,n); % 卡尔曼增益

/ X$ ]7 o' B6 O4 a% kalman filtering

for t=1:n-1
%            s[n]=As*[n-1]+u[n]
% E7 c& e# }& ?  r* a$ J! h%            x[n]=h(s[n])+w[n]
8 J* J7 B  a7 j' A8 A* L
4 s' d0 W# k! T+ t    vx(1,t+1)=vx(1,t)+ux(1,t+1); % 速度过程方程
$ _6 T- G7 t, T    vy(1,t+1)=vy(1,t)+uy(1,t+1); % 速度过程方程
    rx(1,t+1)=rx(1,t)+vx(1,t); % 位置过程方程
    ry(1,t+1)=ry(1,t)+vy(1,t); % 位置过程方程
    s(:,t+1)=[rx(1,t+1);ry(1,t+1);vx(1,t+1);vy(1,t+1)]; % 过程矩阵，维数4*1

    vxe(1,t+1)=vxe(1,t); % 理论精确值
    vye(1,t+1)=vye(1,t); % 理论精确值
9 b" c' C  L& L. {9 M, X1 O    rxe(1,t+1)=rxe(1,t)+vxe(1,t); % 理论精确值
$ e  V( ~1 h" P- o7 q! K6 p* B2 z& Y    rye(1,t+1)=rye(1,t)+vye(1,t); % 理论精确值
    se(:,t+1)=[rxe(1,t+1);rye(1,t+1);vxe(1,t+1);vye(1,t+1)]; % 过程矩阵，维数4*1

" K0 p/ O$ I  _6 g: J% K3 k; M+ Y& t    r(1,t+1)=(rx(1,t+1)^2+ry(1,t+1)^2)^(1/2); % 理论测量距离（含测量误差）
( |3 h, n$ Q- s9 s    b(1,t+1)=atan(ry(1,t+1)/rx(1,t+1)); % 理论测量方位（含测量误差）
! J% C$ r- P. S5 N% p, m$ W! J    rn(1,t+1)=r(1,t+1)+wr(1,t+1); % 实际测量距离
    re(1,t+1)=(rxe(1,t+1)^2+rye(1,t+1)^2)^(1/2); % 理论测量距离（含测量误差）
    bn(1,t+1)=b(1,t+1)+wb(1,t+1); % 实际测量方位
    x(:,t+1)=[rn(1,t+1);bn(1,t+1)]; % 观测矩阵,维数2*1

+ }3 \2 i( |# y% P' @) ^1 ^' x" y    % 对观测方程求导,得到雅可比矩阵H，维数2*4
$ `. f. \9 M" 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];
$ t1 D2 ^. U. w4 \1 _5 f: j, @

    MM(:,:,t+1)=A*M(:,:,t)*A'+Q; % 一步预测均方误差
    K(:,:,t+1)=MM(:,:,t+1)*H(:,:,t+1)'/(C+H(:,:,t+1)*MM(:,:,t+1)*H(:,:,t+1)'); % 卡尔曼增益
& T1 D, o) D8 H- I* I- H: s    M(:,:,t+1)=(eye(4)-K(:,:,t+1)*H(:,:,t+1))*MM(:,:,t+1);% 估计均方误差
    ss(:,t+1)=A*sn(:,t); % 一步预测
    hs(:,t+1)=[((ss(1,t+1))^2+(ss(2,t+1))^2)^(1/2);atan(ss(2,t+1)/ss(1,t+1))]; % 观测值预测
    inn(:,t+1)=x(:,t+1)-hs(:,t+1); % 新息
$ x$ V4 n9 b8 [    sn(:,t+1)=ss(:,t+1)+K(:,:,t+1)*inn(:,t+1); % 预测修正
5 _4 j! P6 y4 P, K) P  Oend

8 o/ P& q+ C. h- @) i$ r2 t: Ksubplot(2,1,1);
plot(sn(1,:),sn(2,:)); % kalman估计值
hold on;
plot(se(1,:),se(2,:),'-r'); % 理论精确值

2 l! T. E8 ^* i* T, }. i9 xsubplot(2,1,2);
plot(rn); % 实际测量距离
+ V5 w  l  E. C7 |" v/ Mhold on;
plot(re,'-g'); % 理论精确值
8 q. L% ?$ q: S; V8 }

HYSYYRPS

这是一个位置和速度跟踪的小程序，感兴趣的可以看一下

HYSYYRPS

有没有同学是电力系统方向的，有的话，有没有做电力系统状态估计方向的，可以交流下