扩展卡尔曼滤波的实现

扩展卡尔曼滤波（Extended Kalman Filter ）与KF的最大的不同，是允许系统模型和测量模型非线性的存在，它的实现较为简单，参照Wikipedia，我把代码贴出来，方便学习交流。

采用一个简单的3阶非线性模型，仿真结果如下图：

源代码：

EKF_Example:

[plain]  view plain copy

1. clc; clear all; close all;  
2. %% system and observation equation, with respect of their Jcobian function.  
3. f_model = @( x, u ) [x(2);x(3);0.05*x(1)*(x(2)+x(3))] + u;  % nonlinear state equations  
4. h_model = @( x ) x(1);                               % measurement equation  
5. f_gradient = @ ( x, u ) ( [ 0 1 0; 0 0 1; 0.05 * ( x(2) + x(3)) 0.05 * x(1) 0.05 * x(1) ] );  
6. h_gradient = @ ( x ) ( [ 1 0 0 ] );  
7. %% Specify initial condition and parameters.  
8. StateDim = 3;      %number of state  
9. ObvDim = 1;  
10. q = 0.1;    %std of process   
11. r = 1.0;    %std of measurement  
12. Q = q^2*eye(StateDim); % covariance of process  
13. R = r^2;        % covariance of measurement    
14. x0_True = [0;0;1];                                % initial state  
15. x0 = x0_True + q * randn(3,1); %initial state          % initial state with noise  
16. L = 1;P0 = L * eye(StateDim);                               % initial state covraiance  
17. MaxIter = 1000;                                     % total dynamic steps  
18. %% EKF.  
19. [ x_Est P_Est ] = EKF( f_model, h_model, f_gradient, h_gradient, x0, P0, Q, R, ObvDim, MaxIter );  

EKF:

[plain]  view plain copy

1. %% Extended Kalman Filter, and the equation is exactly same with Wikipedia(http://en.wikipedia.org/wiki/Extended_Kalman_filter)  
2. %% this is the very basic algorithm, if you want get improved or variants EKF, please change corresponding modular in the following code.  
3. function [ x_Est P_Est ] = EKF( f_model, h_model, f_gradient, h_gradient, x0, P0, Q, R, ObvDim, MaxIter )  
4. % EKF   Extended Kalman Filter for nonlinear dynamic systems  
5. % this function returns state estimate, x and state covariance, P   
6. % for nonlinear dynamic system:  
7. %           x_k = f(x_km1, ukm1) + w_k  
8. %           z_k = h(x_k) + v_k  
9. % where w ~ N(0,Q) meaning w is gaussian noise with covariance Q  
10. %       v ~ N(0,R) meaning v is gaussian noise with covariance R  
11. % Inputs:   f_model: function handle for f( x, u )  
12. %           h_model: function handle for h( x )  
13. %           f_gradient: f'( x, u ), evaluate f_model's gradient at ( x, u )  
14. %           h_gradient: h'( x ), evaluate h_model's gradient at x.  
15. %           x0: "a priori" state estimate  
16. %           P0: "a priori" estimated state covariance  
17. %           Q: process noise covariance of w_k   
18. %           R: measurement noise covariance of v_k  
19. %           ObvDim: z_k's dimension  
20. %           MaxIter: maximum iteration number.  
21. % Output:   x_Est: "a posteriori" state estimate matrix.  
22. %           P_Est: "a posteriori" state covariance matrix.  
23. %  
24. StateDim = length( x0 );  
25. x_True    = [ x0 ];  
26. x_Noise   = [ x0 ];  
27. x_Predict = [ x0 ];  
28. x_Est     = [ x0 ];  
29. u = zeros( StateDim, MaxIter );  
30. z_True    = [];  
31. z_Noise   = [];  
32. P_Predict = [];  
33. P_Est( :, :, 1 ) = P0;  
34. for k = 2 : 1 : MaxIter  
35.     %% this part is only to generate true data and noise data, in practice, you will only get noise data as input.  
36.     % generate true data.  
37.     x_True( :, k ) = f_model( x_True( :, k - 1 ), u( :, k - 1 ) );  
38.     Z_True( :, k ) = h_model( x_True( :, k ) );  
39.     % Add noise, and these are true measurements value.  
40.     Wk = mvnrnd( zeros( StateDim, 1 ), Q );  
41.     x_Noise( :, k ) = x_True( :, k ) + Wk';  
42.     Vk = mvnrnd( zeros( ObvDim, 1 ), R );  
43.     z_Noise( :, k ) = Z_True( :, k ) + Vk';  
44.     %% the following is EKF.  
45.     % Predict, take the last step's state to predict current state.  
46.     x_Predict( :, k ) = f_model( x_Est( :, k - 1 ), u( :, k - 1 ) ); % predicted state estimate.  
47.     F = f_gradient( x_Est( :, k - 1 ), u( :, k - 1 ) );  
48.     P_Predict( :, :, k ) = F * P_Est( :, :, k - 1 ) * F' + Q;  % predict covariance estimate.  
49.     % Update, besides predicted state calculated above, we incorporate   
50.     %measurements to get a more general state estimate.  
51.     y(:, k ) = z_Noise( :, k ) - h_model( x_Predict( :, k ) ); % innovation or measurement residual.  
52.     H = h_gradient( x_Predict( :, k - 1 ) );  
53.     S(:, :, k ) = H * P_Predict( :, :, k ) * H' + R; % innovation covariance.  
54.     K( :, :, k ) = P_Predict( :, :, k ) * H' * inv( S( :, :, k ) ); % Near-optimal Kalman gain.  
55.     x_Est(:, k ) = x_Predict( :, k ) + K( :, :, k ) * y( :, k );    % Update state estimate, this is what you need.  
56.     P_Est( :, :, k ) = ( eye(StateDim) - K( :, :, k ) * H ) * P_Predict( :, :, k ); % update covariance estimate.  
57. end  
58. % RMS error metric.  
59. RMS = mean( ( x_Est - x_True ).^2, 2 )  
60. %% Visualize.  
61. figure(2);  
62. plot( x_True( 1, : ), 'b*' );  
63. hold on;plot( x_Est( 1, : ), 'g*' );  
64. hold on; plot( z_Noise( 1, : ), 'r*' );  
65. legend('True State', 'Optimal State', 'Measurement' );  
66. title( ' Extended Kalman Filter ');  
67. xlabel('Time(s)');ylabel( 'Position(m)' );  
68. grid on;  
69. end