92行目の計算式で「左辺のサイズが1*5で右辺のサイズが5*5であるため,代入を実行できません.」とエラーがでます.全文載せますので,どうすればよいか教えてください.
19 views (last 30 days)
Show older comments
%重力加速度
g = 9.81;
%自転車前後輪位置間隔
L = 1.2;
%ホルダと前輪位置の間隔
d = 0.13;
%自転車ホルダ位置の角度θ
TH = 50;
%走行速度(km/h)
v = 4;
%走行速度(m/s)
V = v* (1000 / 3600 );
%Excelからのデータを数値行列にしてインポートする
DATA = test1;
time = DATA(:,1);
Ax = DATA(:,2);
Ay = DATA(:,3);
Az = DATA(:,4);
Ao = DATA(:,5);
Axp= DATA(:,6);
Ayp= DATA(:,7);
Azp= DATA(:,8);
Aop= DATA(:,9);
%データ数
N = numel(time);
%時間間隔
deltaT = abs(sum(diff(time)))/N;
%各時間に進んだ距離
x = V *deltaT;
%路面プロファイルya用の各時間に進んだ距離の行列
matx1 = V*time;
%路面プロファイルyθ用の各時間に進んだ距離の行列
matx2 = V*time;
%角速度と時間間隔の積
THw = Aop * deltaT;
%推定姿勢角
THwsum = sum(THw);
%Axの最初5個と最後5個の要素を0にする
for K =1:5
Ay(K,:) = 0;
end
for kse =N-5:N
Ay(kse,:) = 0;
end
%重力加速度に由来する比較的長周期の成分を抽出
Aysum = sum(Ay);
%傾斜角
THg =atan(Ay/g);
THgsum = sum(THg);
%鉛直加速度からgを取り除いた動的成分
ay = ((L - d) * Az + d * (Az*(x-L))/L);
%鉛直成分の速度
va = (1/2) * Azp * deltaT;
vasum = sum(va);
%路面プロファイルya
Ya =(1/4) * abs(Ayp) * deltaT^2;
Yasum = sum(Ya);
%推定自転車姿勢角と各時間に進んだ距離からの路面プロファイルyθ
YTH =x*sin(THw);
YTHsum = sum(YTH);
%路面プロファイルyθのグラフ
figure(1),clf
plot(matx2,YTH);
title('自転車姿勢角と各時間に進んだ距離による路面プロファイル')
xlabel('距離(m)');
ylabel('路面の凹凸(cm)');
%路面プロファイルyaのグラフ
figure(2),clf
plot(matx1,Ya);
title('鉛直加速度による路面プロファイル');
xlabel('距離(m)');
ylabel('路面の凹凸(cm)');
deltax = 10*(1000 /3600);
A = [1,deltaT,0,0,0;0,1,0,0,0;0,0,1,deltax,0;0,0,0,1,0;0,0,0,0,0];
B = [0,0,0,0,0;deltaT,0,0,0,0;0,0,0,0,0;0,deltaT,0,0,0;0,0,1,0,0];
C = [1,0,-1,0,0;0,0,0,1,-1];
Q = 1.5*10^-6; R = 1.0*10^-7; U = 5.0*10^-3;
q = randn(N,1)*sqrtm(Q);
r = randn(N,1)*sqrtm(R);
u = randn(N,1)*sqrtm(U);
Z = [q;r;u;0;0];
X = zeros(N,5);
X(1,:) = [0;0;0;0;0];
for k=2:N
X(k,:) =A*X(k-1,:)'+B*Z(k-1,:)'; %個々の計算でエラーが出ます.計算結果を'X'の配列に随時代入したいです.%
end
e1 = X(:,1) + X(:,2);
e2 = X(:,3) + X(:,4);
y1 = z + e1;
y2 = z + e2;
fs = 1;
m = length(e1);
n = pow2(nextpow2(m));
f = (0:n-1)*(fs/n);
ye1 = fft(e1,n);
ye2 = fft(e2,n);
xhat_ukf = zeros(N,5);
xhat = zeros(N,5);
xhat(1,:) = X(1,:)' + [10;10;20];
p = 1000*eye(3);
for k =2:N
[xhat(k,:),p] = kf(A,B,0,C,Q,R,0,y(k),xhat(k-1,:),p);
end
Yhata = Y1 - Y2;
error = xhat-X;
function [xhat_new,p_new, G] = kf(A,B,Bu,C,Q,R,u,y,xhat,p)
xhat =xhat(:); u =u(:); y=y(:);
xhatm = A*xhat + Bu*u;
pm = A*p*A' + B*Q*B';
G = pm*C/(C'*pm*C+R);
xhat_new = xhatm + G*(y-C'*xhatm);
p_new = (eye(size(A))-G*C')*pm;
end
figure(3), clf
エラー: kalmanf (行 92)
X(k,:) =A*X(k-1,:)'+B*Z(k-1,:)';
^^^^^^
0 Comments
Answers (0)
See Also
Categories
Find more on Spreadsheets in Help Center and File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!