创建自定义损失函数以最小化姿态误差

Question

我想使用IMU (加速度计和陀螺仪)读数，通过神经网络计算姿态。输入为input_shape = (time steps, 6)，输出为四元数形式的output_shape = (time steps,4)。

根据数学计算，参考四元数与预测四元数之间的误差为

y_pred[i,]=w0,x0,y0,z0
y_true[i,]=w1,x1,y1,z1 
w = w0*w1 - x0*x1 - y0*y1 - z0*z1
x = w0*x1 + x0*w1 + y0*z1 - z0*y1
y = w0*y1 - x0*z1 + y0*w1 + z0*z1
z = w0*z1 + x0*y1 - y0*x1 + z0*w1
error_quaternion = [w, x, y, z]

为了最小化错误，误差四元数(w)的标度部分必须最小化。(请忽略数学)，为了达到最好的预测，w必须最小化(w是预测和参考姿态之间的最短角度)-

Ref = {Markley，F. Landis和John L. Crassidis.航天器姿态确定和控制的基本原理。第1286卷。美国纽约州纽约:2014年，纽约斯普林格。

我写了这个损失函数

def LossQuat2(y_true, y_pred):
        a, b = y_true.get_shape()
        error = []
        for i in range(a):
            w0,x0,y0,z0 = tf.unstack(y_pred[i,])
            w1,x1,y1,z1 = tf.unstack(y_true[i,])
            x1 = -x1
            y1 = -y1
            z1 = -z1
            w = w0*w1 - x0*x1 - y0*y1 - z0*z1
            error.append(2*tf.math.acos(K.clip(tf.math.sqrt(w*w), -1., 1.)))
        return tf.reduce_mean(error)

为了验证它是否真正计算了错误，我尝试了下面的代码，并精确地计算了错误。

w0,x0,y0,z0 = y_pred[i,]
w1,x1,y1,z1 = y_true[i,]
x1 = -x1
y1 = -y1
z1 = -z1
w = w0*w1 - x0*x1 - y0*y1 - z0*z1
error = 2*math.acos(K.clip(np.sqrt(w*w), -1., 1.))

但使用该损失函数训练模型后，输出误差远大于MSE损失函数。而且，它比MSE慢。

1. 为什么这个损失函数在数学上不能正确地减少错误？
2. 如何减少损失函数的执行时间？
3. 使用for循环函数是否正确？有什么方法可以删除for循环吗？

更新：

数学

四元数

四元数是四元数q=w x y z的姿态表示。

w是标量部分或实数部分。

x y z是向量部分或虚部。

此外，四元数可以写成：

q = [cos(theta/2) e*sin(theta/2)] , e is a unit vector (e=[i j k]

• I打算用神经网络

估计四元数。

四元数逆

四元数逆或四元数共轭可以通过下列方法计算：

quaternion = [w x y z]
inverse(quaternion) = [w -x -y -z]

四元数乘法

为了找出估计姿态和真实(参考)姿态之间的差异，估计姿态(NN输出)必须被四元数乘数乘以四元数基准。

四元数乘法：

q_m = q1 * inverse(q2)

或

q_m = q2 * inverse(q1)

两者都是一样的。

如果

q1=w0,x0,y0,z0
q2=w1,x1,y1,z1 

然后，q_m = [w x y z]和它可以通过以下方法计算：

w = w0*w1 - x0*x1 - y0*y1 - z0*z1
x = w0*x1 + x0*w1 + y0*z1 - z0*y1
y = w0*y1 - x0*z1 + y0*w1 + z0*z1
z = w0*z1 + x0*y1 - y0*x1 + z0*w1

q1和q2之间最短的角度是θ：

Theta = 2*acos(sqrt(w*w))

我需要的是编写一个损失函数来最小化theta，如果θ= 0，w将等于1，那么最优的q_m是：

q_m=[1 0 0 0]

感谢大卫-哈里斯@大卫-哈里斯：

def loss(y_true, y_pred):
    z = y_true * y_pred * tf.constant([[1., -1., -1., -1.]])
    wtot = tf.reduce_sum(z, axis=1)
    return tf.reduce_mean(2*tf.math.acos(tf.math.sqrt(wtot*wtot)))

它的速度要快得多，但它似乎减少了四元数的所有值，因此它不能正确工作。

**

很抱歉给你很多数学机会。

**

更新2

根据David的建议代码，我编写了以下代码：

def loss(y_true, y_pred):
z = y_true * (y_pred * tf.constant([1., -1., -1., -1.000000000]))
wtot = tf.reduce_sum(z,1)
return tf.reduce_mean(2*tf.math.acos(K.clip(tf.math.sqrt(wtot*wtot), -1.,1.)))

此代码减少了损失，但MSE却成倍增长。我知道这段代码不会直接针对MSE进行优化，但是由于数学原因，MSE也必须减少。十世后

loss: 0.0124 - mse: 227.4045 

基于自定义丢失的输出之一

​

Orange = Reference

Blue =用NN估计

基于MSE损失函数的输出之一

​

Answer 1

您应该能够使用这种方法来矢量化(并加快)计算。(我不确定我是否所有的符号都是正确的-不明白为什么你的行“x1=-x1”在那里。)我暂时删除了“剪辑”部分，如果你想要的话，你需要把它放回去)

def loss(y_true, y_pred):
    z = y_true * y_pred * tf.constant([[1., -1., -1., -1.]])
    wtot = tf.reduce_sum(z, axis=1)
    return tf.reduce_mean(2*tf.math.acos(tf.math.sqrt(wtot*wtot)))

我看不出数学上的错误是什么，对不起