Takashi Koyama / HAPS_EKF

EKF class

Dependencies:   MatrixMath Vector3

HAPS_EKF.cpp@2:771eed5f655a, 2021-05-31 (annotated)

Committer:
cocorlow
Date:
Mon May 31 07:27:20 2021 +0000
Revision:
2:771eed5f655a
Parent:
0:de56252b419e 
rpy

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cocorlow 0:de56252b419e 1 #include "HAPS_EKF.hpp"
cocorlow 0:de56252b419e 2 #include "Matrix.h"
cocorlow 0:de56252b419e 3 #include "MatrixMath.h"
cocorlow 0:de56252b419e 4 #include <cmath>
cocorlow 0:de56252b419e 5 #include "Vector3.hpp"
cocorlow 0:de56252b419e 6
cocorlow 0:de56252b419e 7
cocorlow 0:de56252b419e 8 using namespace std;
cocorlow 0:de56252b419e 9
cocorlow 0:de56252b419e 10 HAPS_EKF::HAPS_EKF()
cocorlow 0:de56252b419e 11 :qhat(4,1), qhat_gyro(4,1), Phat(4,4), Qgyro(3,3), Racc(3,3), Rmag(3,3), D(3,3)
cocorlow 0:de56252b419e 12 {
cocorlow 0:de56252b419e 13 qhat << 1 << 0 << 0 << 0;
cocorlow 0:de56252b419e 14
cocorlow 0:de56252b419e 15 Phat.add(1,1,0.05);
cocorlow 0:de56252b419e 16 Phat.add(2,2,0.05);
cocorlow 0:de56252b419e 17 Phat.add(3,3,0.05);
cocorlow 0:de56252b419e 18 Phat.add(4,4,0.05);
cocorlow 0:de56252b419e 19
cocorlow 0:de56252b419e 20 D.add(1,1,1.0);
cocorlow 0:de56252b419e 21 D.add(2,2,1.0);
cocorlow 0:de56252b419e 22 D.add(3,3,1.0);
cocorlow 0:de56252b419e 23
cocorlow 0:de56252b419e 24 Qgyro.add(1,1,0.0224);
cocorlow 0:de56252b419e 25 Qgyro.add(2,2,0.0224);
cocorlow 0:de56252b419e 26 Qgyro.add(3,3,0.0224);
cocorlow 0:de56252b419e 27
cocorlow 0:de56252b419e 28 Racc.add(1,1,0.0330*200);
cocorlow 0:de56252b419e 29 Racc.add(2,2,0.0330*200);
cocorlow 0:de56252b419e 30 Racc.add(3,3,0.0330*200);
cocorlow 0:de56252b419e 31
cocorlow 0:de56252b419e 32 Rmag.add(1,1,1.0);
cocorlow 0:de56252b419e 33 Rmag.add(2,2,1.0);
cocorlow 0:de56252b419e 34 Rmag.add(3,3,1.0);
cocorlow 0:de56252b419e 35 }
cocorlow 0:de56252b419e 36
cocorlow 0:de56252b419e 37 void HAPS_EKF::updateBetweenMeasures(Vector3 gyro, float att_dt)
cocorlow 0:de56252b419e 38 {
cocorlow 0:de56252b419e 39 float q0 = qhat.getNumber( 1, 1 );
cocorlow 0:de56252b419e 40 float q1 = qhat.getNumber( 2, 1 );
cocorlow 0:de56252b419e 41 float q2 = qhat.getNumber( 3, 1 );
cocorlow 0:de56252b419e 42 float q3 = qhat.getNumber( 4, 1 );
cocorlow 0:de56252b419e 43
cocorlow 0:de56252b419e 44 Matrix B(4,3);
cocorlow 0:de56252b419e 45 B << q1 << q2 << q3
cocorlow 0:de56252b419e 46 <<-q0 << q3 <<-q2
cocorlow 0:de56252b419e 47 <<-q3 <<-q0 << q1
cocorlow 0:de56252b419e 48 << q2 <<-q1 <<-q0;
cocorlow 0:de56252b419e 49 B *= 0.5f;
cocorlow 0:de56252b419e 50
cocorlow 0:de56252b419e 51 Matrix phi(4,4);
cocorlow 0:de56252b419e 52 phi << 1.0 << -gyro.x*0.5*att_dt <<-gyro.y*0.5*att_dt <<-gyro.z*0.5*att_dt
cocorlow 0:de56252b419e 53 << gyro.x*0.5*att_dt << 1.0 << gyro.z*0.5*att_dt <<-gyro.y*0.5*att_dt
cocorlow 0:de56252b419e 54 << gyro.y*0.5*att_dt << -gyro.z*0.5*att_dt << 1.0 << gyro.x*0.5*att_dt
cocorlow 0:de56252b419e 55 << gyro.z*0.5*att_dt << gyro.y*0.5*att_dt <<-gyro.x*0.5*att_dt << 1.0;
cocorlow 0:de56252b419e 56
cocorlow 0:de56252b419e 57 qhat = phi*qhat;
cocorlow 0:de56252b419e 58 float qnorm;
cocorlow 0:de56252b419e 59 qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
cocorlow 0:de56252b419e 60 qhat *= (1.0f/ qnorm);
cocorlow 0:de56252b419e 61
cocorlow 0:de56252b419e 62 qhat_gyro = phi*qhat_gyro;
cocorlow 0:de56252b419e 63 qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat_gyro),qhat_gyro));
cocorlow 0:de56252b419e 64 qhat_gyro *= (1.0f/ qnorm);
cocorlow 0:de56252b419e 65
cocorlow 0:de56252b419e 66 Phat = phi*Phat*MatrixMath::Transpose(phi)+B*Qgyro*MatrixMath::Transpose(B);
cocorlow 0:de56252b419e 67
cocorlow 0:de56252b419e 68 q0 = qhat.getNumber( 1, 1 );
cocorlow 0:de56252b419e 69 q1 = qhat.getNumber( 2, 1 );
cocorlow 0:de56252b419e 70 q2 = qhat.getNumber( 3, 1 );
cocorlow 0:de56252b419e 71 q3 = qhat.getNumber( 4, 1 );
cocorlow 0:de56252b419e 72
cocorlow 0:de56252b419e 73 D.add(1,1,q0*q0 + q1*q1 - q2*q2 - q3*q3);
cocorlow 0:de56252b419e 74 D.add(1,2,2*(q1*q2 + q0*q3));
cocorlow 0:de56252b419e 75 D.add(1,3,2*(q1*q3 - q0*q2));
cocorlow 0:de56252b419e 76 D.add(2,1,2*(q1*q2 - q0*q3));
cocorlow 0:de56252b419e 77 D.add(2,2,q0*q0 - q1*q1 + q2*q2 - q3*q3);
cocorlow 0:de56252b419e 78 D.add(2,3,2*(q2*q3 + q0*q1));
cocorlow 0:de56252b419e 79 D.add(3,1,2*(q1*q3 + q0*q2));
cocorlow 0:de56252b419e 80 D.add(3,2,2*(q2*q3 - q0*q1));
cocorlow 0:de56252b419e 81 D.add(3,3,q0*q0 - q1*q1 - q2*q2 + q3*q3);
cocorlow 0:de56252b419e 82 }
cocorlow 0:de56252b419e 83
cocorlow 0:de56252b419e 84 void HAPS_EKF::updateAcrossMeasures(Vector3 _v, Vector3 _u, Matrix& R)
cocorlow 0:de56252b419e 85 {
cocorlow 0:de56252b419e 86 Matrix u(3,1);
cocorlow 0:de56252b419e 87 Matrix v(3,1);
cocorlow 0:de56252b419e 88
cocorlow 0:de56252b419e 89 u << _u.x << _u.y << _u.z;
cocorlow 0:de56252b419e 90 v << _v.x << _v.y << _v.z;
cocorlow 0:de56252b419e 91
cocorlow 0:de56252b419e 92 float q0 = qhat.getNumber( 1, 1 );
cocorlow 0:de56252b419e 93 float q1 = qhat.getNumber( 2, 1 );
cocorlow 0:de56252b419e 94 float q2 = qhat.getNumber( 3, 1 );
cocorlow 0:de56252b419e 95 float q3 = qhat.getNumber( 4, 1 );
cocorlow 0:de56252b419e 96
cocorlow 0:de56252b419e 97 Matrix A1(3,3);
cocorlow 0:de56252b419e 98 A1 << q0 << q3 << -q2
cocorlow 0:de56252b419e 99 <<-q3 << q0 << q1
cocorlow 0:de56252b419e 100 <<q2 <<-q1 <<q0;
cocorlow 0:de56252b419e 101 A1 *= 2.0f;
cocorlow 0:de56252b419e 102
cocorlow 0:de56252b419e 103 Matrix A2(3,3);
cocorlow 0:de56252b419e 104 A2 << q1 << q2 << q3
cocorlow 0:de56252b419e 105 << q2 <<-q1 << q0
cocorlow 0:de56252b419e 106 << q3 <<-q0 <<-q1;
cocorlow 0:de56252b419e 107 A2 *= 2.0f;
cocorlow 0:de56252b419e 108
cocorlow 0:de56252b419e 109 Matrix A3(3,3);
cocorlow 0:de56252b419e 110 A3 <<-q2 << q1 <<-q0
cocorlow 0:de56252b419e 111 << q1 << q2 << q3
cocorlow 0:de56252b419e 112 << q0 << q3 <<-q2;
cocorlow 0:de56252b419e 113 A3 *= 2.0f;
cocorlow 0:de56252b419e 114
cocorlow 0:de56252b419e 115 Matrix A4(3,3);
cocorlow 0:de56252b419e 116 A4 <<-q3 << q0 << q1
cocorlow 0:de56252b419e 117 <<-q0 <<-q3 << q2
cocorlow 0:de56252b419e 118 << q1 << q2 << q3;
cocorlow 0:de56252b419e 119 A4 *= 2.0f;
cocorlow 0:de56252b419e 120
cocorlow 0:de56252b419e 121 Matrix H(3,4);
cocorlow 0:de56252b419e 122
cocorlow 0:de56252b419e 123 Matrix ab1(A1*u);
cocorlow 0:de56252b419e 124 Matrix ab2(A2*u);
cocorlow 0:de56252b419e 125 Matrix ab3(A3*u);
cocorlow 0:de56252b419e 126 Matrix ab4(A4*u);
cocorlow 0:de56252b419e 127
cocorlow 0:de56252b419e 128 H << ab1.getNumber( 1, 1 ) << ab2.getNumber( 1, 1 ) << ab3.getNumber( 1, 1 ) << ab4.getNumber( 1, 1 )
cocorlow 0:de56252b419e 129 << ab1.getNumber( 2, 1 ) << ab2.getNumber( 2, 1 ) << ab3.getNumber( 2, 1 ) << ab4.getNumber( 2, 1 )
cocorlow 0:de56252b419e 130 << ab1.getNumber( 3, 1 ) << ab2.getNumber( 3, 1 ) << ab3.getNumber( 3, 1 ) << ab4.getNumber( 3, 1 );
cocorlow 0:de56252b419e 131
cocorlow 0:de56252b419e 132
cocorlow 0:de56252b419e 133 Matrix K(4,3);
cocorlow 0:de56252b419e 134 K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
cocorlow 0:de56252b419e 135
cocorlow 0:de56252b419e 136 Matrix dq(4,1);
cocorlow 0:de56252b419e 137 dq = K*(v-D*u);
cocorlow 0:de56252b419e 138 qhat = qhat+dq;
cocorlow 0:de56252b419e 139
cocorlow 0:de56252b419e 140 float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
cocorlow 0:de56252b419e 141 qhat *= (1.0f/ qnorm);
cocorlow 0:de56252b419e 142
cocorlow 0:de56252b419e 143 Matrix eye4(4,4);
cocorlow 0:de56252b419e 144 eye4 << 1 << 0 << 0 << 0
cocorlow 0:de56252b419e 145 << 0 << 1 << 0 << 0
cocorlow 0:de56252b419e 146 << 0 << 0 << 1 << 0
cocorlow 0:de56252b419e 147 << 0 << 0 << 0 << 1;
cocorlow 0:de56252b419e 148 Phat = (eye4-K*H)*Phat*MatrixMath::Transpose(eye4-K*H)+K*R*MatrixMath::Transpose(K);
cocorlow 0:de56252b419e 149 }
cocorlow 0:de56252b419e 150
cocorlow 2:771eed5f655a 151 void HAPS_EKF::computeAngles(Vector3& rpy, Vector3& rpy_g, Vector3 rpy_align)
cocorlow 0:de56252b419e 152 {
cocorlow 0:de56252b419e 153 float q0 = qhat.getNumber( 1, 1 );
cocorlow 0:de56252b419e 154 float q1 = qhat.getNumber( 2, 1 );
cocorlow 0:de56252b419e 155 float q2 = qhat.getNumber( 3, 1 );
cocorlow 0:de56252b419e 156 float q3 = qhat.getNumber( 4, 1 );
cocorlow 2:771eed5f655a 157 rpy.x = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2)-rpy_align.x;
cocorlow 2:771eed5f655a 158 rpy.y = asinf(-2.0f * (q1*q3 - q0*q2))-rpy_align.y;
cocorlow 2:771eed5f655a 159 rpy.z = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
cocorlow 0:de56252b419e 160
cocorlow 0:de56252b419e 161 q0 = qhat_gyro.getNumber( 1, 1 );
cocorlow 0:de56252b419e 162 q1 = qhat_gyro.getNumber( 2, 1 );
cocorlow 0:de56252b419e 163 q2 = qhat_gyro.getNumber( 3, 1 );
cocorlow 0:de56252b419e 164 q3 = qhat_gyro.getNumber( 4, 1 );
cocorlow 2:771eed5f655a 165 rpy_g.x = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2)-rpy_align.x;
cocorlow 2:771eed5f655a 166 rpy_g.y = asinf(-2.0f * (q1*q3 - q0*q2))-rpy_align.y;
cocorlow 2:771eed5f655a 167 rpy_g.z = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
cocorlow 0:de56252b419e 168
cocorlow 0:de56252b419e 169 }
cocorlow 0:de56252b419e 170
cocorlow 0:de56252b419e 171 void HAPS_EKF::triad(Vector3 fb, Vector3 fn, Vector3 mb, Vector3 mn){
cocorlow 0:de56252b419e 172 Matrix W1(3,1);
cocorlow 0:de56252b419e 173 W1 << fb.x << fb.y << fb.z;
cocorlow 0:de56252b419e 174 Matrix W2(3,1);
cocorlow 0:de56252b419e 175 W2 << mb.x << mb.y << mb.z;
cocorlow 0:de56252b419e 176
cocorlow 0:de56252b419e 177 Matrix V1(3,1);
cocorlow 0:de56252b419e 178 V1 << fn.x << fn.y << fn.z;
cocorlow 0:de56252b419e 179 Matrix V2(3,1);
cocorlow 0:de56252b419e 180 V2 << mn.x << mn.y << mn.z;
cocorlow 0:de56252b419e 181
cocorlow 0:de56252b419e 182
cocorlow 0:de56252b419e 183 Matrix Ou2(3,1);
cocorlow 0:de56252b419e 184 Ou2 << W1.getNumber( 2, 1 )*W2.getNumber( 3, 1 )-W1.getNumber( 3, 1 )*W2.getNumber( 2, 1 ) << W1.getNumber( 3, 1 )*W2.getNumber( 1, 1 )-W1.getNumber( 1, 1 )*W2.getNumber( 3, 1 ) << W1.getNumber( 1, 1 )*W2.getNumber( 2, 1 )-W1.getNumber( 2, 1 )*W2.getNumber( 1, 1 );
cocorlow 0:de56252b419e 185 Ou2 *= 1.0/sqrt(MatrixMath::dot(MatrixMath::Transpose(Ou2),Ou2));
cocorlow 0:de56252b419e 186 Matrix Ou3(3,1);
cocorlow 0:de56252b419e 187 Ou3 << W1.getNumber( 2, 1 )*Ou2.getNumber( 3, 1 )-W1.getNumber( 3, 1 )*Ou2.getNumber( 2, 1 ) << W1.getNumber( 3, 1 )*Ou2.getNumber( 1, 1 )-W1.getNumber( 1, 1 )*Ou2.getNumber( 3, 1 ) << W1.getNumber( 1, 1 )*Ou2.getNumber( 2, 1 )-W1.getNumber( 2, 1 )*Ou2.getNumber( 1, 1 );
cocorlow 0:de56252b419e 188 Ou3 *= 1.0/sqrt(MatrixMath::dot(MatrixMath::Transpose(Ou3),Ou3));
cocorlow 0:de56252b419e 189 Matrix R2(3,1);
cocorlow 0:de56252b419e 190 R2 << V1.getNumber( 2, 1 )*V2.getNumber( 3, 1 )-V1.getNumber( 3, 1 )*V2.getNumber( 2, 1 ) << V1.getNumber( 3, 1 )*V2.getNumber( 1, 1 )-V1.getNumber( 1, 1 )*V2.getNumber( 3, 1 ) << V1.getNumber( 1, 1 )*V2.getNumber( 2, 1 )-V1.getNumber( 2, 1 )*V2.getNumber( 1, 1 );
cocorlow 0:de56252b419e 191 R2 *= 1.0/sqrt(MatrixMath::dot(MatrixMath::Transpose(R2),R2));
cocorlow 0:de56252b419e 192 Matrix R3(3,1);
cocorlow 0:de56252b419e 193 R3 << V1.getNumber( 2, 1 )*R2.getNumber( 3, 1 )-V1.getNumber( 3, 1 )*R2.getNumber( 2, 1 ) << V1.getNumber( 3, 1 )*R2.getNumber( 1, 1 )-V1.getNumber( 1, 1 )*R2.getNumber( 3, 1 ) << V1.getNumber( 1, 1 )*R2.getNumber( 2, 1 )-V1.getNumber( 2, 1 )*R2.getNumber( 1, 1 );
cocorlow 0:de56252b419e 194 R3 *= 1.0/sqrt(MatrixMath::dot(MatrixMath::Transpose(R3),R3));
cocorlow 0:de56252b419e 195
cocorlow 0:de56252b419e 196 Matrix Mou(3,3);
cocorlow 0:de56252b419e 197 Mou << W1.getNumber( 1, 1 ) << Ou2.getNumber( 1, 1 ) << Ou3.getNumber( 1, 1 )
cocorlow 0:de56252b419e 198 << W1.getNumber( 2, 1 ) << Ou2.getNumber( 2, 1 ) << Ou3.getNumber( 2, 1 )
cocorlow 0:de56252b419e 199 << W1.getNumber( 3, 1 ) << Ou2.getNumber( 3, 1 ) << Ou3.getNumber( 3, 1 );
cocorlow 0:de56252b419e 200 Matrix Mr(3,3);
cocorlow 0:de56252b419e 201 Mr << V1.getNumber( 1, 1 ) << R2.getNumber( 1, 1 ) << R3.getNumber( 1, 1 )
cocorlow 0:de56252b419e 202 << V1.getNumber( 2, 1 ) << R2.getNumber( 2, 1 ) << R3.getNumber( 2, 1 )
cocorlow 0:de56252b419e 203 << V1.getNumber( 3, 1 ) << R2.getNumber( 3, 1 ) << R3.getNumber( 3, 1 );
cocorlow 0:de56252b419e 204
cocorlow 0:de56252b419e 205 Matrix Cbn = Mr*MatrixMath::Transpose(Mou);
cocorlow 0:de56252b419e 206
cocorlow 0:de56252b419e 207 float sqtrp1 = sqrt(1.0+Cbn.getNumber( 1, 1 )+Cbn.getNumber( 2, 2 )+Cbn.getNumber( 3, 3 ));
cocorlow 0:de56252b419e 208
cocorlow 0:de56252b419e 209 qhat.add(1,1,0.5*sqtrp1);
cocorlow 0:de56252b419e 210 qhat.add(2,1,-(Cbn.getNumber( 2, 3 )-Cbn.getNumber( 3, 2 ))/2.0/sqtrp1);
cocorlow 0:de56252b419e 211 qhat.add(3,1,-(Cbn.getNumber( 3, 1 )-Cbn.getNumber( 1, 3 ))/2.0/sqtrp1);
cocorlow 0:de56252b419e 212 qhat.add(4,1,-(Cbn.getNumber( 1, 2 )-Cbn.getNumber( 2, 1 ))/2.0/sqtrp1);
cocorlow 0:de56252b419e 213
cocorlow 0:de56252b419e 214 float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
cocorlow 0:de56252b419e 215 qhat *= (1.0f/ qnorm);
cocorlow 0:de56252b419e 216
cocorlow 0:de56252b419e 217 qhat_gyro = qhat;
cocorlow 0:de56252b419e 218
cocorlow 0:de56252b419e 219 float q0 = qhat.getNumber( 1, 1 );
cocorlow 0:de56252b419e 220 float q1 = qhat.getNumber( 2, 1 );
cocorlow 0:de56252b419e 221 float q2 = qhat.getNumber( 3, 1 );
cocorlow 0:de56252b419e 222 float q3 = qhat.getNumber( 4, 1 );
cocorlow 0:de56252b419e 223
cocorlow 0:de56252b419e 224 D.add(1,1,q0*q0 + q1*q1 - q2*q2 - q3*q3);
cocorlow 0:de56252b419e 225 D.add(1,2,2*(q1*q2 + q0*q3));
cocorlow 0:de56252b419e 226 D.add(1,3,2*(q1*q3 - q0*q2));
cocorlow 0:de56252b419e 227 D.add(2,1,2*(q1*q2 - q0*q3));
cocorlow 0:de56252b419e 228 D.add(2,2,q0*q0 - q1*q1 + q2*q2 - q3*q3);
cocorlow 0:de56252b419e 229 D.add(2,3,2*(q2*q3 + q0*q1));
cocorlow 0:de56252b419e 230 D.add(3,1,2*(q1*q3 + q0*q2));
cocorlow 0:de56252b419e 231 D.add(3,2,2*(q2*q3 - q0*q1));
cocorlow 0:de56252b419e 232 D.add(3,3,q0*q0 - q1*q1 - q2*q2 + q3*q3);
cocorlow 0:de56252b419e 233 }
cocorlow 0:de56252b419e 234
cocorlow 0:de56252b419e 235 Vector3 HAPS_EKF::calcMagRef(Vector3 m)
cocorlow 0:de56252b419e 236 {
cocorlow 0:de56252b419e 237 float _x, _y, _z;
cocorlow 0:de56252b419e 238 Matrix magvec(3,1);
cocorlow 0:de56252b419e 239 magvec << m.x << m.y << m.z;
cocorlow 0:de56252b419e 240 Matrix magnedvec = MatrixMath::Transpose(D)*magvec;
cocorlow 0:de56252b419e 241 _x = sqrt(magnedvec(1,1)*magnedvec(1,1)+magnedvec(2,1)*magnedvec(2,1));
cocorlow 0:de56252b419e 242 _y = 0.0f;
cocorlow 0:de56252b419e 243 _z = magnedvec(3,1);
cocorlow 0:de56252b419e 244 return Vector3(_x, _y, _z);
cocorlow 0:de56252b419e 245 }
cocorlow 0:de56252b419e 246
cocorlow 0:de56252b419e 247 Vector3 HAPS_EKF::calcDynAcc(Vector3 LPacc, Vector3 accref)
cocorlow 0:de56252b419e 248 {
cocorlow 0:de56252b419e 249 float _x, _y, _z;
cocorlow 0:de56252b419e 250 _x = LPacc.x-(D.getNumber( 1, 1 )*accref.x+D.getNumber( 1, 2 )*accref.y+D.getNumber( 1, 3 )*accref.z);
cocorlow 0:de56252b419e 251 _y = LPacc.y-(D.getNumber( 2, 1 )*accref.x+D.getNumber( 2, 2 )*accref.y+D.getNumber( 2, 3 )*accref.z);
cocorlow 0:de56252b419e 252 _z = LPacc.z-(D.getNumber( 3, 1 )*accref.x+D.getNumber( 3, 2 )*accref.y+D.getNumber( 3, 3 )*accref.z);
cocorlow 0:de56252b419e 253 return Vector3(_x, _y, _z);
cocorlow 0:de56252b419e 254 }