扩展卡尔曼滤波(Extended Kalman Filter )与KF的最大的不同,是允许系统模型和测量模型非线性的存在,它的实现较为简单,参照Wikipedia,我把代码贴出来,方便学习交流。
采用一个简单的3阶非线性模型,仿真结果如下图:
源代码:
EKF_Example:
- clc; clear all; close all;
- %% system and observation equation, with respect of their Jcobian function.
- f_model = @( x, u ) [x(2);x(3);0.05*x(1)*(x(2)+x(3))] + u; % nonlinear state equations
- h_model = @( x ) x(1); % measurement equation
- f_gradient = @ ( x, u ) ( [ 0 1 0; 0 0 1; 0.05 * ( x(2) + x(3)) 0.05 * x(1) 0.05 * x(1) ] );
- h_gradient = @ ( x ) ( [ 1 0 0 ] );
- %% Specify initial condition and parameters.
- StateDim = 3; %number of state
- ObvDim = 1;
- q = 0.1; %std of process
- r = 1.0; %std of measurement
- Q = q^2*eye(StateDim); % covariance of process
- R = r^2; % covariance of measurement
- x0_True = [0;0;1]; % initial state
- x0 = x0_True + q * randn(3,1); %initial state % initial state with noise
- L = 1;P0 = L * eye(StateDim); % initial state covraiance
- MaxIter = 1000; % total dynamic steps
- %% EKF.
- [ x_Est P_Est ] = EKF( f_model, h_model, f_gradient, h_gradient, x0, P0, Q, R, ObvDim, MaxIter );
EKF:
- %% Extended Kalman Filter, and the equation is exactly same with Wikipedia(http://en.wikipedia.org/wiki/Extended_Kalman_filter)
- %% this is the very basic algorithm, if you want get improved or variants EKF, please change corresponding modular in the following code.
- function [ x_Est P_Est ] = EKF( f_model, h_model, f_gradient, h_gradient, x0, P0, Q, R, ObvDim, MaxIter )
- % EKF Extended Kalman Filter for nonlinear dynamic systems
- % this function returns state estimate, x and state covariance, P
- % for nonlinear dynamic system:
- % x_k = f(x_km1, ukm1) + w_k
- % z_k = h(x_k) + v_k
- % where w ~ N(0,Q) meaning w is gaussian noise with covariance Q
- % v ~ N(0,R) meaning v is gaussian noise with covariance R
- % Inputs: f_model: function handle for f( x, u )
- % h_model: function handle for h( x )
- % f_gradient: f'( x, u ), evaluate f_model's gradient at ( x, u )
- % h_gradient: h'( x ), evaluate h_model's gradient at x.
- % x0: "a priori" state estimate
- % P0: "a priori" estimated state covariance
- % Q: process noise covariance of w_k
- % R: measurement noise covariance of v_k
- % ObvDim: z_k's dimension
- % MaxIter: maximum iteration number.
- % Output: x_Est: "a posteriori" state estimate matrix.
- % P_Est: "a posteriori" state covariance matrix.
- %
- StateDim = length( x0 );
- x_True = [ x0 ];
- x_Noise = [ x0 ];
- x_Predict = [ x0 ];
- x_Est = [ x0 ];
- u = zeros( StateDim, MaxIter );
- z_True = [];
- z_Noise = [];
- P_Predict = [];
- P_Est( :, :, 1 ) = P0;
- for k = 2 : 1 : MaxIter
- %% this part is only to generate true data and noise data, in practice, you will only get noise data as input.
- % generate true data.
- x_True( :, k ) = f_model( x_True( :, k - 1 ), u( :, k - 1 ) );
- Z_True( :, k ) = h_model( x_True( :, k ) );
- % Add noise, and these are true measurements value.
- Wk = mvnrnd( zeros( StateDim, 1 ), Q );
- x_Noise( :, k ) = x_True( :, k ) + Wk';
- Vk = mvnrnd( zeros( ObvDim, 1 ), R );
- z_Noise( :, k ) = Z_True( :, k ) + Vk';
- %% the following is EKF.
- % Predict, take the last step's state to predict current state.
- x_Predict( :, k ) = f_model( x_Est( :, k - 1 ), u( :, k - 1 ) ); % predicted state estimate.
- F = f_gradient( x_Est( :, k - 1 ), u( :, k - 1 ) );
- P_Predict( :, :, k ) = F * P_Est( :, :, k - 1 ) * F' + Q; % predict covariance estimate.
- % Update, besides predicted state calculated above, we incorporate
- %measurements to get a more general state estimate.
- y(:, k ) = z_Noise( :, k ) - h_model( x_Predict( :, k ) ); % innovation or measurement residual.
- H = h_gradient( x_Predict( :, k - 1 ) );
- S(:, :, k ) = H * P_Predict( :, :, k ) * H' + R; % innovation covariance.
- K( :, :, k ) = P_Predict( :, :, k ) * H' * inv( S( :, :, k ) ); % Near-optimal Kalman gain.
- x_Est(:, k ) = x_Predict( :, k ) + K( :, :, k ) * y( :, k ); % Update state estimate, this is what you need.
- P_Est( :, :, k ) = ( eye(StateDim) - K( :, :, k ) * H ) * P_Predict( :, :, k ); % update covariance estimate.
- end
- % RMS error metric.
- RMS = mean( ( x_Est - x_True ).^2, 2 )
- %% Visualize.
- figure(2);
- plot( x_True( 1, : ), 'b*' );
- hold on;plot( x_Est( 1, : ), 'g*' );
- hold on; plot( z_Noise( 1, : ), 'r*' );
- legend('True State', 'Optimal State', 'Measurement' );
- title( ' Extended Kalman Filter ');
- xlabel('Time(s)');ylabel( 'Position(m)' );
- grid on;
- end