What is the possible error in the state estimator?

조회 수: 2 (최근 30일)
Rajath Pai
Rajath Pai 2019년 8월 26일
답변: Rajath Pai 2019년 9월 1일
% Constants
G = 6.671e-11; %[Nm^2kg^-2] : Universal gravitational constant
M_E = 5.97e24; %[kg] : Mass of Earth
H = 7e5; %[m] : Given altitude of satellite
R_E = 6378e3; %[m] : Radius of Earth
a = H + R_E; %[m] : Distance of the satelltie
% from the center of the Earth
n = sqrt(G*M_E/a^3); % : Mean motion of the satellite
% Spacecraft parameters provided
T_d = [ 0.001 ;
0.001;
0.001]; % [Nm] Disturbance torques
J = diag([2500,2300,3000]);
% Intial conditions
theta_in = deg2rad([10 10 10]); % [rad] Initial attitude angle error
omega_in = deg2rad([6 2 4]); % [rad/s] Initial angular velocity
theta_rel = [0 0 0]'; % [rad] Reference/ Desired attitude
t = 0;
t_i = 0 ; % [s] Initial time
t_f = 100; % [s] Final time
dt = 0.01 ; % [s] time-step
D1 = (t_f-t_i)/dt;
theta = theta_in(1:3)'; % Initial attitude angles
omega = omega_in(1:3)'; % Initial angular velocity
state = [theta' omega']; % Initial state
T_c = 0 ;
T_t = T_d + TGG(theta',n,J) + T_c ; % Total torque acting on the body
omega_dot(1:3,1) = dynamics(state,J,T_t);
theta_dot(1:3,1) = Kinematics(state);
theta_err(1:3,1) = theta_rel - theta(1:3,1);
b = [0.1 -0.1 0.15]'*pi/180 ;
% Gains and accuracy
theta_acc = deg2rad(0.1);% Steady state error
Kp = [1500 1500 1500]; % Proportional gain
Kd = [400 400 400 ];
acc_abs = 0.1*ones(D1,1);
for i = 2:D1
w(1:3,i) = deg2rad(0.1)*randn(3,1);
THT = theta(1:3,i-1) + theta_dot(1:3,i-1)*dt + w(1:3,i); % Adding random noise to attitude angle measurement
OMG = omega(1:3,i-1) + omega_dot(1:3,i-1)*dt ;
theta(1:3,i) = THT ;
omega(1:3,i) = OMG ;
omegabias(1:3,i) = OMG + b;
biasstate = [THT' omegabias(1:3,i)'];
state(1:3) = THT;state(4:6) = OMG ;
t(i) = t(i-1)+dt ;
theta_err(1:3,i) = theta_rel - theta(1:3,i);
T_c(1:3,i) = Controller(theta_err,Kp,Kd,dt,i);
T_t(1:3,i) = T_ext + TGG(theta',n,J) + T_c(1:3,i) ;
theta_dot(1:3,i) = Kinematics(biasstate);
omega_dot(1:3,i) = dynamics(state,J,T_t(1:3,i));
end
% State estimator check
b_est(1:3,1) = deg2rad([1 ; 1 ; 1 ]) ; % Intialising Estimated bias
x_kk(1:6,1) = [10*theta(1:3,1); b_est(1:3,1)];
P_kk = diag(deg2rad([10 10 10 -9.9 9.9 9.9]).^2) ;%10000*eye(6); % Inital state predictor error covariance matrix P_(k,k)
noise = deg2rad(0.1); % White-Gaussian noise standard deviation
R = diag([noise^2 noise^2 noise^2]); % Measurement noise covariance matrix
Q = diag([noise^2 noise^2 noise^2 noise^2 noise^2 noise^2 ]); % System noise covariance matrix
for j = 1:D1
[x_kk(1:6,j+1), p_kk,F] = fcn(x_kk(1:6,j),omegabias(1:3,j),theta(1:3,j),P_kk,dt,n,Q,R);
P_kk = p_kk ;
end
theta_fil = rad2deg(x_kk(1:3,:));
b_est = rad2deg(x_kk(4:6,:));
function[theta_dot] = Kinematics(state)
n = 0.001059782633307 ; % Mean motion
theta = state(1:3);
omega = state(4:6);
theta_dot = ([cos(theta(2)) sin(theta(1))*sin(theta(2)) cos(theta(1))*sin(theta(2));
0 cos(theta(2))*cos(theta(1)) -cos(theta(2))*sin(theta(1));
0 sin(theta(1)) cos(theta(1)) ]*[omega(1) ; omega(2) ; omega(3)] + n*[sin(theta(3)) ; cos(theta(2))*cos(theta(3)) ; sin(theta(2))*cos(theta(3))])/cos(theta(2));
end
function[omega_dot] = dynamics(state,J,T)
omega = state(4:6)';
Omega = [0 -omega(3) omega(2); omega(3) 0 -omega(1); -omega(2) omega(1) 0];
omega_dot = -inv(J)*Omega*J*omega + inv(J)*T;
end
function [x_dot,f] = system_eqn(x_kk,u_m,n_orb)
% This function is used to compute the system equation x_dot = f+G*u.
% It takes the state vector x_kk, the omega measured wm and the
% mean motion n_orb for inputs and computes the state vector derivative.
tht1 = x_kk(1); % theta_x
tht2 = x_kk(2); % theta_y
tht3 = x_kk(3); % theta_z
b1 = x_kk(4); % bias_x
b2 = x_kk(5); % bias_y
b3 = x_kk(6); % bias_z
w1m = u_m(1); % omega_x
w2m = u_m(2); % omega_y
w3m = u_m(3); % omega_z
thtd1 = -b1 -b2*sin(tht1)*tan(tht2) -b3*cos(tht1)*tan(tht2)+ (n_orb*sin(tht3))/cos(tht2);
thtd2 = -b2*cos(tht1)+b3*sin(tht1)+n_orb*cos(tht3);
thtd3 = (-b2*sin(tht1)-b3*(cos(tht1))/cos(tht2))+ n_orb*tan(tht2)*cos(tht3);
f = [thtd1; thtd2; thtd3; 0; 0; 0]; % f(x(t),t)
G = [1 sin(tht1)*tan(tht2) cos(tht1)*tan(tht2); % g(x(t),t)
0 cos(tht1) -sin(tht1);
0 (sin(tht1)/cos(tht2)) (cos(tht1)/cos(tht2));
0 0 0;
0 0 0;
0 0 0];
u = [w1m w2m w3m]'; % omega_measured vector
x_dot = f+G*u; % Nonlinear state space model
end
function [x_kk, p_kk,K_k_1] = fcn(x_kk,wm, theta_m,p_kk,dt,n,Q,R)
% This function is the algorithm for the Kalman filter. It estimates the
% next step elements (state vector x_kk and error covariance matrix p_kk)
% from the previous step.
% Observation matrix
H = [1 0 0 0 0 0;
0 1 0 0 0 0;
0 0 1 0 0 0];
tht1 = x_kk(1); % Attitude angle along the x-axis
tht2 = x_kk(2); % Attitude angle along the y-axis
tht3 = x_kk(3); % Attitude angle along the z-axis
b1 = x_kk(4); % Gyro bias along the x-axis
b2 = x_kk(5); % Gyro bias along the y-axis
b3 = x_kk(6); % Gyro bias along the z-axis
[x_dot] = system_eqn(x_kk, wm,n);
% Jacobian matrix of the function f of the system
F = [ tan(tht2)*(-b2*cos(tht1)+b3*sin(tht1)), ( -b2*sin(tht1) -b3*cos(tht1)+ n*sin(tht3)*sin(tht2))/cos(tht2)^2, n*cos(tht3)/cos(tht2), -1 , -sin(tht1)*tan(tht2), -cos(tht1)*tan(tht2) ;
b2*sin(tht1) + b3*cos(tht1) , 0 , -n*sin(tht3) , 0 , -cos(tht1) , sin(tht1) ;
(-b2*cos(tht1)+ b3*sin(tht1))/cos(tht2) , ((-b2*sin(tht1)-b3*cos(tht1))*sin(tht2)+ n*cos(tht3))/cos(tht2)^2 , -n*tan(tht2)*sin(tht3) , 0 , -sin(tht1)/cos(tht2) , -cos(tht1)/cos(tht2) ;
0 , 0 , 0 , 0 , 0 , 0 ;
0 , 0 , 0 , 0 , 0 , 0 ;
0 , 0 , 0 , 0 , 0 , 0 ] ;
G = eye(6) ;
[Phi,Gamma] = c2d(F,G,dt) ; % System transition matrix
x_k_1k = x_kk + x_dot * dt; % Step 1: Time propagation
p_k_1k = Phi*p_kk*Phi' + Gamma*Q*Gamma'; % Step 2 : covariance amtrix update
K_k_1 = p_k_1k * H' /(H*p_k_1k*H' + R); % Step 3: Kalman gain
x_k_1k_1 = x_k_1k + K_k_1*(theta_m - H*x_k_1k); % Step 4: Measurement update
pk_1k_1 = (eye(6)-K_k_1*H)*p_k_1k *(eye(6)-K_k_1*H)'+K_k_1*R*K_k_1';% Step 5
x_kk = x_k_1k_1; % State vector update
p_kk = pk_1k_1; % Error covariance matrix update
end
function[T_GG] = TGG(theta,n,J)
a31 = -sin(theta(2));
a32 = sin(theta(1))*cos(theta(2));
a33 = cos(theta(1))*cos(theta(2));
T_GG = 3*n^2*[0 -a33 a32; a33 0 -a31; -a32 a31 0]*J*[a31 ; a32 ; a33]; % [Nm] Gravity gradient torque
The above is my code to generate a noisy attitude and angular velocity measurements. I've tried to integrate it with a state estimator, but there seem to be errors which isn't in the math. One of the things I've found is that my b_est does not converge to the added bias 'b', which is one of the states that the estimator must estimate. While my theta's getting filtered, the biases don't converge to the bias values but just converge towards 0.
I see it as perhaps an error of initialisation or implementation in matlab and thus have turned towards the community to help identify potential errors. I'm not new to MATLAB, but I am yet a beginner, so I'd appreciate any and all possible helpful advice.
  댓글 수: 4
KALYAN ACHARJYA
KALYAN ACHARJYA 2019년 8월 29일
Provide the details of all variables, like you have used theta, have you defined it anywhere in the code?
Rajath Pai
Rajath Pai 2019년 8월 29일
I have provided all the details now. You cna have a look and let me know if you see any errors or mistakes and/or explain what could be happening wrong.

댓글을 달려면 로그인하십시오.

채택된 답변

Rajath Pai
Rajath Pai 2019년 9월 1일
The error was in the modeling itself. The usage of theta_m in Kinematics state isn't accurate and thus was creating issues. Thank you for helping me out.

추가 답변 (1개)

Mahesh Taparia
Mahesh Taparia 2019년 8월 29일
Hi,
You have not defined some variables in your code, and some are not properly defined. Do the following update:
  • Value of ‘J’ is not defined.
  • Write the following line before the ‘a31’ expression,
theta = [theta_in(1) ; theta_in(2) ; theta_in(3)];
  • T_ext (in line 26) is not defined in your code.
  • You have defined T_GG variable while you are using TGG.
  • Check all the variables in the workspace if they are not defined in your code, define it and use it with same name.
  댓글 수: 5
Mahesh Taparia
Mahesh Taparia 2019년 8월 30일
I guess this is algorithmic part, check your code as per your algo.
Rajath Pai
Rajath Pai 2019년 9월 1일
Understood thanks.

댓글을 달려면 로그인하십시오.

카테고리

Help CenterFile Exchange에서 General Applications에 대해 자세히 알아보기

제품


릴리스

R2017a

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by