├── CKF.m ├── GenerateScenario.m ├── Main.m ├── MstEq.m ├── Predict.m ├── README.md ├── Update.m └── mydate.m /CKF.m: -------------------------------------------------------------------------------- 1 | classdef CKF 2 | 3 | properties 4 | 5 | Q; 6 | R; 7 | 8 | nx; 9 | 10 | xkk1; 11 | Pkk1; 12 | 13 | xkk; 14 | Pkk; 15 | 16 | 17 | 18 | end 19 | 20 | methods 21 | 22 | % Constructor 23 | function obj = CKF(Q, R, nx) 24 | obj.Q = Q; 25 | obj.R = R; 26 | obj.nx = nx; 27 | end 28 | 29 | 30 | % Propagation 31 | function obj = Predict(obj, xkk, Pkk) 32 | 33 | obj.xkk1 = xkk; 34 | 35 | obj.Pkk1 = Pkk + obj.Q; 36 | end 37 | 38 | % Correction 39 | function obj = Update(obj, z) 40 | 41 | R = obj.R; 42 | xkk1 = obj.xkk1; 43 | Pkk1 = obj.Pkk1; 44 | 45 | %%%======================================================================== 46 | %%% Genrate a set of Cubature Points 47 | %%%======================================================================== 48 | 49 | nx = obj.nx; % state vector dimension 50 | 51 | nPts = 2*nx; % No. of Cubature Points 52 | 53 | CPtArray = sqrt(nPts/2)*[eye(nx) -eye(nx)]; 54 | 55 | %%%======================================================================== 56 | %%% find the square-root of Pkk1 using Singular Value Decomposition (SVD) 57 | %%%======================================================================== 58 | 59 | Pkk1 = 0.5*(Pkk1+Pkk1'); % make Pkk1 to be symmetric (Property of a covariance matrix !) 60 | 61 | [U, S, V] = svd(Pkk1); 62 | 63 | Skk1 = 0.5*(U+V)*sqrt(S); 64 | 65 | %%%======================================================================== 66 | 67 | Xi = repmat(xkk1,1,nPts) + Skk1*CPtArray; 68 | 69 | Zi = MstEq(Xi); 70 | 71 | zkk1 = sum(Zi,2)/nPts; % predicted Measurement 72 | 73 | X = (Xi-repmat(xkk1,1,nPts))/sqrt(nPts); 74 | 75 | Z = (Zi-repmat(zkk1,1,nPts))/sqrt(nPts); 76 | 77 | Pzz = Z*Z'+ R; % Innovations Covariance 78 | 79 | Pxz = X*Z'; % cross-covariance 80 | 81 | G = Pxz*pinv(Pzz); % Cubature Kalman Gain 82 | 83 | xkk = xkk1 + G*(z - zkk1); 84 | 85 | Pkk = Pkk1 - G*Pzz*G'; 86 | 87 | % Save objects 88 | obj.xkk = xkk; 89 | obj.Pkk = Pkk; 90 | 91 | end 92 | end 93 | 94 | 95 | 96 | methods(Static) 97 | 98 | function z = MstEq(x) 99 | 100 | z = [ 0.8*cos(x(1,:)) - 0.2*cos(x(1,:) + x(2,:)) ; 101 | 0.8*sin(x(1,:)) - 0.2*sin(x(1,:) + x(2,:)) ]; 102 | end 103 | end 104 | 105 | end -------------------------------------------------------------------------------- /GenerateScenario.m: -------------------------------------------------------------------------------- 1 | function [x,y] = GenerateScenario(Q) 2 | 3 | Q1 = Q(1:2,1:2); 4 | r1 = 0.8; 5 | r2 = 0.2; 6 | 7 | noise = Q1 * randn(2,1000); 8 | 9 | theta(1) = 0; 10 | theta2(1) = 0; 11 | 12 | t=1; 13 | count = 1; 14 | 15 | for j = 0.3:.1:1.2 16 | for k = pi/2:.05:1.5*pi 17 | theta1(t) = j + noise(1,t); 18 | if (-1)^count == -1; 19 | theta2(t) = k + noise(2,t); 20 | else 21 | theta2(t) = 2*pi - k + noise(2,t); 22 | end 23 | xx(t) = r1*cos(theta1(t)) - r2*cos(theta1(t)+theta2(t)); 24 | yy(t) = r1*sin(theta1(t)) - r2*sin(theta1(t)+theta2(t)); 25 | t = t + 1; 26 | end 27 | count = count + 1; 28 | end 29 | x = [theta1;theta2]; % 2-by-630 30 | y = [xx; yy]; % 2-by-630 31 | -------------------------------------------------------------------------------- /Main.m: -------------------------------------------------------------------------------- 1 | %%%======================================================================= 2 | %%% This matlab code implements the CKF 3 | %%%======================================================================= 4 | 5 | clear; 6 | clc; 7 | close all; 8 | 9 | nx = 2; % state vector dimension 10 | nState = nx; 11 | 12 | nExpt = 10; % No. of Experiments/Trials 13 | 14 | N = 630; % No. of Time steps 15 | 16 | %%% Process Noise Covariance Q 17 | sigma1 = 1e-2; 18 | sigma2 = 1e-1; 19 | Q = diag([sigma1^2 sigma2^2 ]); 20 | 21 | %%% Measurement Noise Covariance R 22 | R = 0.005*eye(nState); 23 | 24 | % Initialise metric store 25 | MSE = zeros(nState, N); 26 | estMSE = zeros(nState, N); 27 | 28 | xestArray = zeros(nState, N); 29 | 30 | % Initial covariance prior 31 | Skk = diag([0.9 pi/6]); 32 | 33 | 34 | % Generate Testcase 35 | [xArray,zArray] = GenerateScenario(Q); 36 | 37 | 38 | 39 | %% Iterate 40 | for expt = 1:nExpt 41 | 42 | % Initial mean and covariance prior 43 | xkk = [0.3+0.9*rand; pi/2+pi*rand]; 44 | Pkk = Skk*Skk'; 45 | 46 | ckfObj = CKF(Q, R, nx); 47 | 48 | fprintf('MC Run in Process = %d\n',expt); 49 | 50 | % Iterate for evert measurement 51 | for k = 1:N 52 | 53 | % Propagate estimate and covariance 54 | ckfObj = ckfObj.Predict(xkk, Pkk); 55 | 56 | % Update estimate and covariance 57 | z = zArray(:,k); % measurement 58 | ckfObj = ckfObj.Update(z); 59 | 60 | % Get outputs 61 | xkk = ckfObj.xkk; 62 | Pkk = ckfObj.Pkk; 63 | 64 | % Save state estimate 65 | xestArray(:,k) = xkk; 66 | 67 | % True value 68 | xTrue = xArray(:, k); 69 | 70 | % Calculate metrics 71 | MSE(:,k)= MSE(:,k) + sum((xTrue - xkk).^2); 72 | 73 | estMSE(:,k) = estMSE(:,k)+ trace(Pkk); 74 | 75 | end; % time-step 76 | 77 | end; % expts 78 | 79 | MSE = MSE/(2*nExpt); 80 | estMSE = estMSE/(2*nExpt); 81 | 82 | RMSE = MSE.^(0.5); 83 | estRMSE = estMSE.^(0.5); 84 | 85 | %%%======================================================================== 86 | %%% Plotting 87 | %%%======================================================================== 88 | 89 | figure; 90 | subplot(2,1,1); 91 | plot(xArray(1,:),'k'); 92 | hold on; 93 | plot(xestArray(1,:),'r:'); 94 | ylabel('y(m)','fontsize',16); 95 | legend('Actual','CKF',2); 96 | hold off; 97 | 98 | subplot(2,1,2); 99 | plot(xArray(2,:),'k'); 100 | hold on; 101 | plot(xestArray(2,:),'r:'); 102 | xlabel('Time, k','fontsize',12); 103 | ylabel('y(m)','fontsize',16); 104 | legend('Actual','CKF',2); 105 | hold off; 106 | 107 | figure; 108 | x = 1:N; 109 | semilogy(x,RMSE(1,:),'r'); 110 | hold on; 111 | semilogy(x,estRMSE(1,:),'r:'); 112 | legend('RMSE','filter est. RMSE'); 113 | ylabel('RMSE','fontsize',16); 114 | xlabel('Time, k','fontsize',12); 115 | 116 | 117 | -------------------------------------------------------------------------------- /MstEq.m: -------------------------------------------------------------------------------- 1 | function z = MstEq(x) 2 | 3 | z = [ 0.8*cos(x(1,:)) - 0.2*cos(x(1,:) + x(2,:)) ; 4 | 0.8*sin(x(1,:)) - 0.2*sin(x(1,:) + x(2,:)) ]; 5 | -------------------------------------------------------------------------------- /Predict.m: -------------------------------------------------------------------------------- 1 | function [xkk1,Pkk1] = Predict(xkk,Pkk,Q) 2 | 3 | xkk1 = xkk; 4 | 5 | Pkk1 = Pkk + Q; 6 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ckf-matlab 2 | -------------------------------------------------------------------------------- /Update.m: -------------------------------------------------------------------------------- 1 | function [xkk,Pkk] = Update(xkk1,Pkk1,z, R) 2 | 3 | %%%======================================================================== 4 | %%% Genrate a set of Cubature Points 5 | %%%======================================================================== 6 | 7 | nx = 2; % state vector dimension 8 | 9 | nPts = 2*nx; % No. of Cubature Points 10 | 11 | CPtArray = sqrt(nPts/2)*[eye(nx) -eye(nx)]; 12 | 13 | %%%======================================================================== 14 | %%% find the square-root of Pkk1 using Singular Value Decomposition (SVD) 15 | %%%======================================================================== 16 | 17 | Pkk1 = 0.5*(Pkk1+Pkk1'); % make Pkk1 to be symmetric (Property of a covariance matrix !) 18 | 19 | [U, S, V] = svd(Pkk1); 20 | 21 | Skk1 = 0.5*(U+V)*sqrt(S); 22 | 23 | %%%======================================================================== 24 | 25 | Xi = repmat(xkk1,1,nPts) + Skk1*CPtArray; 26 | 27 | Zi = MstEq(Xi); 28 | 29 | zkk1 = sum(Zi,2)/nPts; % predicted Measurement 30 | 31 | X = (Xi-repmat(xkk1,1,nPts))/sqrt(nPts); 32 | 33 | Z = (Zi-repmat(zkk1,1,nPts))/sqrt(nPts); 34 | 35 | Pzz = Z*Z'+ R; % Innovations Covariance 36 | 37 | Pxz = X*Z'; % cross-covariance 38 | 39 | G = Pxz*pinv(Pzz); % Cubature Kalman Gain 40 | 41 | xkk = xkk1 + G*(z - zkk1); 42 | 43 | Pkk = Pkk1 - G*Pzz*G'; 44 | 45 | 46 | -------------------------------------------------------------------------------- /mydate.m: -------------------------------------------------------------------------------- 1 | classdef mydate 2 | % write a description of the class here. 3 | properties 4 | % define the properties of the class here, (like fields of a struct) 5 | minute = 0; 6 | hour; 7 | day; 8 | month; 9 | year; 10 | end 11 | methods 12 | % methods, including the constructor are defined in this block 13 | function obj = mydate(minute,hour,day,month,year) 14 | % class constructor 15 | if(nargin > 0) 16 | obj.minute = minute; 17 | obj.hour = hour; 18 | obj.day = day; 19 | obj.month = month; 20 | obj.year = year; 21 | end 22 | end 23 | function obj = rollDay(obj,numdays) 24 | obj.day = obj.day + numdays; 25 | end 26 | end 27 | end --------------------------------------------------------------------------------